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Chapter 1 

Introduction 


Thi£s<tocument£0p6rts on the>¥Q^Wonejijji«f^ASA Coo^ratiye 
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oratory underNJjiije'tlirection of Professor Robert H. Cannoftnfc.) The goal of this research is 
to develop and test new control techniques for self-contained, autonomous free-flying space 
robots. Free-flying space robots are envisioned as a key element of any successful long term 
presence in space. These robots must be capable of performing the assembly, maintenance, 
and inspection, and repair tasks that currently require astronaut extra-vehicular activity 
(EVA). Use of robots will provide economic savings as well as improved astronaut safety 
by reducing and in many cased eliminating the need for human EVA. 

The focus of ovr work is to develop and carry out a set of research projects using labora- 
tory models of satellite robots. These devices use air-cushion- vehic le (AC V) technology to 
simulate in t wo dimensions the drag-free, zero-g conditions of space. / Usingtwo large grairttg" 

which serve as t he platfo rm' fnr thr-n — r^prrirrirrit" 

we are able to reduce gravitv-i mllli ml MiTmr tn iind^r in~ 5 g with a co rresponding 

drag-to^eiglrt iatk> oTabout 10 -4 — a very good approximation to the actual cone 
Space. 

■Ow current work is divided into six major projects or research areas* CeupeidliVe Mi- 
nipnlatioa ™ n p n . n Mempttfartion o r e=Ftoatmg fta se ; 6fobai- 

Navifrat.inn inH Print . nl .<. f a FmuiTPIl m I Iwp - Wliln H Tufnl l l p ln U . tiinln Tnoporatinn | ^n ajtgT- 


" a1iiv° ftaraport -sae ■ ■ 11 ,J| I T FAP ~ (fl 7 l | ,,T m il ti ii ii Bntemcemeirt ,r in AgBft-Frafc-Qfffc 
Adaptive CoiLli Ul Of LEAP. 1 

Fixed-base cooperative manipulation work represents our initial entry into multiple arm 
cooperation and high-level control with a sophisticated user interface. T - h re- ocporimonfc ic 
now fu ll y on line and - ha s- al r eady producod sev e ral s i gnificant n ew rcoultO r 

The floating-base cooperative manipulation pro ject (strives? to transfer some of the new 
technologies developed in the fixed-base work onto a floating base. Thin experimont will b e 
u e inf-^nr gfuirnUirm 'ipnm mhnfr mnrlr l which it util l under rnmtn i rlii^n i Fyppri 


m a n te inrlnrla grasping and mntlipl l lni i I l j_ ,1 ftPSttllg llllj l l " l ii iin^ I flo nti nc b nn r , 

The global control and navigation experiment seeks to demonstrate simultaneous control 
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CHAPTER 1. INTRODUCTION 


of the robot manipulators and the robot base position so that tasks can be accomplished 
while the base is undergoing a controlled motion. 

Multiple- vehicle cooperation ptojed ffia. new activity that was started during th i o rop ort 
period: The goal thia- project ie^to demonstrate multiple free-floating robots working 
in teams to carry out tasks too difficult or complex for a single robot to perform. -A- 
t hirds vpWIp, fiirnilnr tn tVw. r.w n Vininff KnHt far tlwi T F.A.E prajort | is alsn rurranl lu under 

co nstructi o n; and it will bring the flee t uf air cushion v e hicles to - t hree. ^ 

The LEAP activity sw an -s tarted about a yoar agorw&h the goal -of provid»| a viable 
alternative to expendable gas thrusters for vehicle propulsion wherein the robot uses its 
manipulators to throw itself from place to place. Thia - work will bo carried out with a sl i g htly 
rev is e d - versi o n of se cond g e ner a tion spare robot which i s currently und e r construction . 

Adap tivo LEAP projoct io a*hcw activity that wao otartod du r ing thc-laot rop o rt perio d. 
Because the successful execution of the LEAP technique requires an accurate model of the 
robot and payload mass properties, it was deemed an attractive testbed for adaptive control 
technology. Init i al s t ud i es arp underway , to evaluate - various adaptive control algorit hms. 

T he chaptore that f o llow give detuilod pr o gress a n d- status re po r ts , on. A-project-by- 
pxojecl basis. 
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Chapter 2 


Fixed-Base Cooperative 
Manipulation Experiment 


Stan Schneider 


2.1 Introduction 

To accelerate our development of multi-armed, free-flying satellite manipulators, we have 
developed a fixed-base cooperative manipulation facility. Although the manipulator arms 
are fixed to a rigid base, they manipulate free-flying objects. This facility allows us to 
experiment quickly with cooperative algorithms, expediting our study of space-based ma- 
nipulation and assembly. This section describes the progress made to date in our research 
on cooperative manipulation. 

2.1.1 Progress Summary 

During this report period, much of the theory developed over the last three years was 
implemented. In particular, the object impedance control, state table programming, real- 
time vision, and graphical user interface modules were installed and demonstrated. 

Our initial research effort into cooperative manipulation is now complete. The vision 
system is capable of identifying, tracking, and capturing a moving object. We have demon- 
strated high-performance cooperative control, both in “free” motion, and when the manip- 
ulated object is in contact with its environment. Automatic capture, docking (connector 
insertion), withdrawal, and throwing functions are supported by the strategic command 
module. The mouse-based graphical user interface allows an operator to direct the activ- 
ities of the system at the conceptual level. The operator commands only object motions; 
the arm actions required to effect these motions need not be specified. This design allows 
simple specification of many tasks; in particular, simple assembly operations can be easily 
accomplished. Each of these functions has been fully demonstrated. 

More specifically, the major accomplishments of the period March, 1988 through Au- 
gust, 1988 were: 
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• Implemented and demonstrated dual arm cooperative object impedance control. This 
was shown to be an effective dynamic control methodology for cooperative manipu- 
lation. 

• Implemented the real-time vision system point tracking algorithm. 

• Developed algorithms for identification and tracking of moving objects. Demonstrated 
vision-guided dual-arm intercept and capture. 

• Implemented and demonstrated a novel robot programming methodology — state table 
progr ammin g. This forms the heart of the strategic control module. 

• Designed and installed an interactive graphical user interface to provide conceptual 
level system command. 

• Demonstrated the operational cooperative system under user control. 

Not all of our proposed approaches were successful; the client-server real-time software 
structuring concept [6] was finally abandoned as impractical. A simpler — and more suc- 
cessful — software structuring approach was implemented; it is outlined below. 


2.1.2 Background: Research Goals 

Space construction requires the manipulation of large, delicate objects. Single manipula- 
tor arms are incapable of quickly maneuvering these objects without exerting large local 
torques. Multiple cooperating arms do not suffer from this limitation. Unfortunately, co- 
operative robotic manipulation technology is not yet well understood. The goal of this 
project is to study the problem of cooperative manipulation in a weightless environment, 
and to demonstrate experimentally a cooperative robotic assembly. 

Four aspects are to be studied in detail: 

• The dynamic control of multiple arm manipulation systems 

• The utilization of video “vision” data for real-time control 

• Real-time software structuring for cooperative robotic systems 

• User interfacing: the acquisition and utilization of strategic commands 


2.2 Facility Development 

2.2.1 Mechanical Hardware 

The fixed-base cooperation facility consists of a pair of two- link manipulators, affixed to 
the side of a “small” granite table (4 feet x 8 feet). Each arm is of the popular SCARA 
configuration — basically anthropomorphic, with vertical-axis, revolute “shoulder” and “el- 
bow” joints. The arms are capable of motion in the plane of the table, and can interact 
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with objects floating on air-cushions on the granite surface. This facility is essentially 
complete — no major additions or modifications occurred during the report period. 

2.2.2 Computer System 

Our real-time computer system combines a proven UNIX development environment with 
high-performance real-time processing hardware. Motorola 68020/68881 single board pro- 
cessors running the pSOS real-time kernel provide inexpensive real-time processing power. 
VME bus shared-memory communications permit efficient multiprocessor operation. The 
real-time processors are linked, via the VME bus, to our Sun/3 engineering workstations. 
Thus, we benefit from Sun’s superb programming environment, while providing the ca- 
pacity for relatively cheap, unlimited processing expansion. This system is also essentially 
complete. Only evolutionary modifications to the various software modules were performed 
during the report period. 

2.2.3 Calibration 

During the last period, automated sensor calibration programs were developed for: joint 
angular positions, joint pseudo- velocities, endpoint forces, and motor torque outputs. Dur- 
ing this period, the vision system was installed as a major new sensor system. This posed 
two new calibration problems: calibration of the vision system itself, and sensor fusion with 
the endpoint information provided by the joint angle sensors. Since these are both logically 
vision system issues, they are discussed in the vision system section below. 
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2.3 Multiprocessor Real-time Software Environment 

During this report period, the client-server real-time structuring methodology described in 
the original proposal was abandoned. A simpler — and more successful — software structur- 
ing approach was implemented. This section discusses the reasons for this action. This 
section also presents an overview of the software structure as implemented in the “final” 
operational system. 

2.3.1 Real-time software environment 

Each real-time processor runs the pSOS kernel. The pSOS kernel is a small, fast, priority- 
driven multi-tasking kernel. The features used most heavily by our software structure are 
the multi-tasking scheduler, the inter-process message facility, and the event-signal facility. 
The interested reader should consult [35] for details. 


2.3.2 Client-Server Structuring 

According to the client-server paradigm, the real-time software is divided into small, in- 
dependently executing modules, each with a well-defined function in the controller data 
flow path. The modules communicate their data to other modules via message passing. 
For example, one processor (the server) might be assigned to read and process the analog 
inputs. This module is responsible for pre-processing the incoming data, and maintaining 
its integrity. Client processes — possibly running on a remote CPU — request information of 
the server when they require it. The pre-processed data is then sent via a message to the 
client process. A significant advantage of this scheme is that changes to the analog (server) 
environment are well isolated from the client code — the system is highly modular. 

As noted in the previous report, the biggest disadvantage with this structure is the 
loading on the server — the processor responsible for reading and pre-processing the analog 
inputs. The fixed-base facility has a three-processor computer system. The processing load 
divides naturally as one processor to calculate each arm’s dynamics, etc., and one processor 
for “system” level tasks, such as vision, object dynamics, etc. With the vision system and 
the object impedance controller active, the “system” processor is already heavily loaded. 
Using it also to pre-process sensor data is simply not reasonable. 

A second disadvantage is the communication overhead incurred. As noted in the last 
report, we were able to reduce this to an acceptable level. However, the shared-memory 
structure outlined below proved far superior regardless. The authors feel that client-server 
structuring is still attractive for some situations: when independent processing power is 
available to dedicate to sensor processing, sensors require significant pre-processing, small 
delays in data presentation are not significant, and the extra overhead is tolerable. With 
the exception of the vision system, none of these conditions exists for the processing load of 
this system. It was judged better to adapt the vision system to a simpler structure, rather 
than diminish the performance of the entire system. 
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2.3.3 Shared-Memory Structuring 

Instead of the client-server data structuring, a much simpler global shared-memory struc- 
ture was adopted. Under this paradigm, the controller data flow paths are always executed 
by a single stream of execution, on a single processor. Thus, there is no data communica- 
tion overhead within the loop. Data communications to external modules must still occur; 
these are accomplished via simple global data structures. An interprocessor lock gate is 
provided with each structure. Each communicating module is responsible for obtaining the 
lock before accessing the data. 

Note that command or temporal information flow is not efficient with shared data struc- 
tures. This type of communication is handled via communication “channels,” described 
below. 

This scheme has several disadvantages. First, it obscures the responsibility for main- 
taining the integrity of the data. This did not prove to be a significant problem for our 
system. A more secure system (although not nearly as secure as the client-server system) 
would result from installing active (i.e. callable subroutine) modules in each processor to 
mediate the data access. This would result in some additional overhead; it should not be 
significant. 

Second, the shared-memory scheme is less modular. Changes to any part of the global 
data structure affect every module that accesses any part of the structure. Worse, any 
module that “breaks the rules” and, for instance, gets the lock and holds it, can bring the 
whole system a halt. Of course, both these effects could be ameliorated by installing active 
mediation modules. 

This scheme does, however, have one redeeming quality; it’s fast. If each accessing 
module keeps its access time small, communication overhead is practically eliminated. It 
also fits the processing load of the fixed-base facility nicely — the “arm” processors are 
relatively lightly loaded compared to the “system”processor, and can easily handle the 
additional load of reading and processing the analog inputs. 

2.3.4 Inter-processor Communications 

While simple shared-memory structures are sufficient for data communication, other types 
of information must also be transmitted. For instance, temporal information (“the data 
in structure xxx is ready now”), and commands (“switch to independent arm operation 
mode” or “catch that object”) are difficult to convey with global data structures. To handle 
this type of information flow, we have also installed an inter- processor (or inter-process) 
communications facility, called “channels”. A channel is simply a byte-stream two-way data 
flow — analogous to Unix “sockets” or “pipes” — with optional interrupt notification. 

Channels can be created by any process (on any processor) in the system, and attached 
(opened) by any other process. Two mechanisms are available for a receiving process to 
obtain data from a channel. First, if the data is temporally urgent, the receiving process 
can indicate that it will monitor apSOS kernel “event”, and check the channel immediately 
when the event (and its associated interrupt) occurs. The sending process can then issue 
a special command to the channel to cause the receiver to be activated. Second, the 
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receiver may simply poll the channel at its leisure to determine if any new data is present. 
This paradigm allows the receiver to control the rate of data flow. Combinations of these 
techniques are explicitly allowed; the receiver may, for instance, wait for an event, but 
only for a fixed period of time. The sender thus has the option of sending messages in the 
background or indicating that an urgent need has arisen at any time. 


2.4 Software structure 

This section outlines the system software structure. The reader should refer to Figure 2.1 
throughout this discussion. 

The cooperating arms application software consists of four major modules. The modules 
axe: dynamic control, vision, strategic control, and user interface. Execution of these 
modules is spread over three real-time 68020 processors, and the Sun graphical workstation. 

The dynamic control module is responsible for reading the various sensors and calcu- 
lating the actuator torques required to produce the desired system behavior. This module 
is further divided into three sub-modules as shown in Figure 2.1: the object impedance 
controller, and the two arm dynamic controllers. 

The vision module is responsible for interpreting the incoming video pixel data, and 
disseminating the object and arm endpoint positions. 

The strategic control module is responsible for the overall command of the system. It 
fields high-level requests from the user interface module, and translates them into sequences 
of primitives that the dynamic control module can implement. It also monitors the various 
conditions and activities of the system and directs appropriate response actions. 

Finally, the user interface collect conceptual-level commands from the operator, and 
communicate them to the strategic controller. 


User 


A 



Figure 2.1: System Structure 
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2.4.1 Structure Chart Format 


More detailed analysis of each module’s structure is presented with the module descriptions 
below. The design is multi-process in nature; concurrency is employed to help modularize 
the code and permit asynchronous operation. To aid the reader in understanding the 
inter-process relationships, software structure charts will be presented in each section. 

The structure charts utilize a common shape-encoding, presented in Figure 2.2. Rounded 
corner boxes denote any general “module”, any set of related subroutines. Module routines 
may be accessed by many processes asynchronously. Square boxes with rounded tops denote 
independently executing processes. Rounded tops with the letters “ISP” indicate execu- 
tion flow within interrupt service procedures. Named square boxes indicate inter- processor 
shared memory structures. 

Example meanings of arrows connecting the boxes are also indicated in Figure 2.2. The 
arrows may have different meanings in different contexts 1 . For instance, an arrow into a 
module box indicates a subroutine call, a transaction representing flow of both information 
and execution control. An arrow out of a process box indicates a routine call-out. Arrows 
into process boxes indicate asynchronous information transfers: messages being sent to the 
process or the signaling of an event. Arrows into data structures indicate writes into the 
structure, arrows out of the structure indicate data reads. 


Module 


Process 


Interrupt 

Service 

Procedure 


Shared Data 
Structure 




Cali oration 


r n 

Menu 

Driver 



Sample _ 


Arms 


Routine 

Call-in 


Asynchronous 
Access (msg, event) 


Routine 

Call-out 


Data 

Write 


Figure 2.2: Structure Chart Key 


J This is unfortunate, but multiple arrow types make the figures very complex. 
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2.5 Strategic Control 

The strategic control module was also implemented during this report period 2 . The strate- 
gic control module is based on a finite state machine programming technique. It provides 
the user interface with a set of simple automated commands, allowing it to perform many 
simple tasks. 

2.5.1 Strategic Control as a Module in the Larger System 

The purpose of the strategic controller is to provide an interface between the conceptual 
commands provided by the user interface 3 and the dynamic control module. An effective 
strategic control methodology has several important features: it encourages modular pro- 
gramming design, it allows simple specification of the actions required to complete the 
desired tasks, and it facilitates intuitive task programming. A strategic control module for 
a cooperative system must also be able to synchronize the motions of the manipulators. 
In addition, conceptual requests should be communicated as simply as possible, and the 
results of those requests should be reported promptly. 

To be effective, the strategic controller must have access to a large variety of system 
data and activities. It must monitor the motions of the various objects in the workspace, 
control the activities of the manipulators, and interact with the conceptual interface to the 
user. In short, although strategic control clearly fits into the command hierarchy between 
dynamic control and conceptual command, from a data-flow perspective it spans all the 
levels. 

2.5.2 Traditional Robotic Programming Techniques 

Traditional robot programming takes one of three forms: teach programming, specialized 
robot programming languages, and explicit use of general-purpose languages. This section 
examines some of the merits and problems associated with each method. 

Teach programming Teach programming requires the programmer to move the robot 
through the desired motions. The system then “plays back” the sequence of motions. 
Teaching is only effective for position control tasks. It is impossible to specify synchro- 
nization with other moving obstacles or manipulators. Several attempts to improve the 
means of teaching have been made, including one with cooperating robots [15]. These all 
still suffer from the basic weakness: it is very difficult to specify complex motions, time 
dependencies, and interacting conditions. 

Special robot programming languages Several robot-specific programming languages 
are in use. Examples are VAL [33] and IBM’s AML [38]. These languages allow the 
programmer to specify the manipulator’s motion as a series of instructions. Motions that 
require waiting for a condition must be programmed as a loop, such as: 


2 The underlying theory was developed over the last few years. 
3 or an artificially intelligent command module 
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repeat { 

Move in the -X direction 0.01 meters; 

} until ( Force X exceeds 2 Newtons ) 

This is a considerable improvement in the types of tasks that can be programmed; force 
conditions are also specifiable. Note that force control is not available; the best that can 
be done is to check for force levels that exceed given values. 

Multiple conditions pose a more significant problem. The only recourse with standard 
programming techniques is to test for all conditions in the loop, as in: 

repeat { 

Move in the -X direction 0.01 meters; 

} until ( (Force X exceeds 2 Newtons) or (X < 0.2 meters) ) 


This quickly becomes unwieldy. If, for example, a sequence of moves must be completed 
in a limited period of time, each move in the sequence must incorporate the time-out test 
in its execution loop. As the tests become more sophisticated — and the sequences of moves 
more complex — this results in very confusing, hard to maintain software. An even more 
vexing problem is created if the condition test takes a long time to execute. The programmer 
is left with two bad choices: execute the test every loop and slow the motion considerably, 
or execute it every n th loop (another messy addition to the loop code), and produce uneven 
motion. 

IBM’s AML language does allow the setting of overall error condition tests. If the error 
condition arises, a special code segment is executed to handle the condition. Unfortunately, 
normal occurrences of multiple pending conditions must be dealt with as above; there is no 
facility for changing the flow of execution asynchronously. 

The LM robot programming system [23] implements a considerably more powerful set 
of constructs to support concurrent execution. A facility is provided to start a motion while 
the “main” execution flow continues. Synchronization primitives permit changing a motion 
in progress, or waiting for completion. In addition, LM defines a “guarded command” 
structure to address the multiple pending conditions problem. A parallel-execution facility 4 
is also provided. The intent is to provide the ability to specify multiple programs for multiple 
robots. 

While it is clear that LM provides a large set of parallel programming facilities, it does 
not provide the programmer with a simple structure to assist in managing the concurrency 
in the system. An integrated system involving multiple manipulators, real-time vision, and 
interactive operator control is a complex, event-driven environment. With a fundamentally 
sequential underlying programming paradigm, the burden of managing these asynchronous 
events is left to the programmer. For instance, “guarded commands” allow changing of the 
course of a trajectory. However, to change the flow of execution of the “main” program when 
an outstanding pending condition occurs requires sprinkling conditional tests throughout 


4 This implements a “cobegin { <blocklist> } coend;” structure, similar to Concurrent Pascal. 
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the “main” execution flow. 

In addition, while the multiple-parallel-block execution structure is appropriate for coor- 
dinating multiple robots, it is considerably less attractive for execution of a tightly-coupled 
parallel control algorithm — such as a cooperative dynamic controller. Switching to a coop- 
erative control mode upon the occurrence of some event is particularly awkward. 

In summary, synchronization of multiple cooperating manipulators handling a single 
object is very difficult with traditional robot programming languages. Although a few 
attempts have been made [43], they consider only coordinated position control policies, 
and thus are of little practical use. 

General-purpose language programming There have also been several implemen- 
tations of motion control libraries written to facilitate programming in general-purpose 
languages [14]. These libraries usually provide useful data structure definitions — such as 
“transform” — and motion primitives to allow specification of motion sequences. 

Although some concurrency issues are addressed, these libraries do not directly aid the 
programmer in utilizing the concurrency in the system. All still rely on a basically sequential 
user-code execution model. In RCCL [14], for example, the robot program consists of 
one main “user” process, and one background “control-task” process. The “user” process 
executes a sequential series of instructions, and communicates them to the “control-task” 
process, executing at the control loop sample rate. The only synchronization provided is 
via a “waitfor” primitive; no support for independently executing user-written processes is 
provided. 

As a result, these implementations do not alleviate the multiple pending conditions 
problem. None provide support for multiple loops, nor multiple sample rates. 

In addition, no support is provided for direct interfacing with other large real-time 
modules, such as sophisticated dynamic controllers, or vision systems 5 . These systems are 
naturally concurrent in nature; complex asynchronous interactions muSt be managed. A 
sequential execution stream model can not, for instance, deal effectively with multiple- 
arm synchronization, especially if the arms are running different programs different 
processors. 

2.5.3 The state table programming technique 

This section presents an alternative programming methodology, referred to as state table 
programming. It is a form of explicit programming in a general-purpose language, but 
provides a structure the previous attempts lack. 

The state table programming technique provides a coherent structure to guide the 
programmer in producing code that is easily interfaced to other real-time system modules. 
It directly exploits the facile multiple-process generation and communications provided 
by modern real-time kernels. In fact, management of multiple asynchronous processes is 
central to the structure of the system; the programmer is actively encouraged to divide the 

5 There are several integrated vision positioning and inspection interfaces [42, 24], but they do not address 
the real-time vision issues. 
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problem into small, independently executing programs. In addition, it is based on a very 
intuitive task description technique. 

The state table programming technique is not a replacement for a library of robot control 
routines. Trajectory generation utilities and world modeling utilities are still required. The 
technique merely provides a frame- work that weaves the multiple streams of execution in 
the system into an easy-to-use structure. 


State transition graphs State transition graphs provide a simple, intuitive means of vi- 
sualizing the sequence of actions required to effect a task. A “state” is defined operationally: 
the system is always in some state, a new state is entered whenever the set of conditions 
(stimuli) the system can process changes. A state is usually characterized by the system 
performing a single operation, and waiting for some indication of its completion 6 . State 
transitions are indicated by arrows labeled with the event that causes the transition. When 
an event occurs, a transition routine is executed. The result of that routine determines the 
next state entered. 

For example, Figure 2.3 presents a simplified series of steps required to catch a moving 
object. The system starts out in the “Idle” state. The receipt of the “Acquire” stimulus, 
presumably sent from the conceptual level, causes the system to enter one of two states: 
“Waiting” if the object is out of reach, or “Reaching”, if not. While the system is in the 
“Reaching” state, the arms are executing an intercept trajectory. When the trajectory 
completes (“TrajComplete” event) , the arms track the object until the gripper endpoints 
match the targeted grip ports precisely. At this point, the system enters the “Gripping” 
state while the grippers engage. Finally, the catch is complete, and the “Manipulating” 
state is active until a “Release” command is received. 



6 The example below should help clarify this concept. 
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State 

Stimulus 

Transition Routine 

Next States 

Idle 

Acquire 

CheckRange 

Reaching 

Waiting 

Waiting 

Time 

CheckRange 

Reaching 

Waiting 

Reaching 

TrajComplete 

CoordinatedMode 

Tracking 


Tracking 

InterceptOK 

GrippersDown 

Gripping 



Time 

AbortAcquire 

Idle 


Gripping 

Time 

CooperativeMode 

Manipulating 


Manipulating 

Release 

GrippersUp 

Idle 



Table 2.1: State Transition Table 


This is a rather simple example. Much more complex series of actions — with multiple 
branches — are easily representable. 


The state table programming technique The state table programming method pro- 
vides a simple means to implement the series of actions indicated in the state transition 
graph. A state table is constructed directly from the information in the state transition 
graph. Associated with each state are the events (stimuli) understood in that state. Each 
stimulus has a transition routine, and a list of “next states” that will be entered based on 
the value returned by the transition routine. 

The state table entries corresponding to the states of Figure 2.3 are presented in ta- 
ble 2.1. The “Idle” state in this example understands only the “Acquire” stimulus. When 
“Acquire” is received, the routine “CheckRange” will execute. If “CheckRange” finds the 
object to be “in range”, then “Reaching” will be the next state, otherwise, the next state 
will be “Waiting”. Note that the same transition routine, “CheckRange”, can be used in 
several different contexts. Its job is simple: check on the location of the object, and report 
the outcome. Note also that multiple pending conditions are handled very naturally, for 
example, the “Tracking” state waits for either “InterceptOK” or “Time”. 

Stimuli may be generated by any of the modules of the system, but most originate 
from one of two sources: command stimuli from the user interface, and condition-event 
stimuli from independently executing “helpers”. The “Acquire” and “Release” stimuli 
are examples of the former. Several examples of the latter also occur in table 2.1. The 
“Time” stimulus is created by a simple process that simply sleeps for a specified time before 
sending its message. The “InterceptOK” stimulus is sent by a process that executes in the 
background, and simply checks to see if both arms match the gripper port positions. Other 
helpers (not shown in the table) perform “safety checks”; for example, the “OutOfRange” 
stimulus is sent by a process that checks every so often to injure that the object being 
acquired is not suddenly moved out of range. 

Unexpected stimuli may be handled in either of two manners. First, if no stimulus 
matches the incoming stimulus, a default error handling routine is executed. Second, the 
special stimulus “AnyStimulus” may appear as the last entry in any state’s table entry. All 
stimuli match this entry. The transition routine associated with “AnyStimulus” may then 
process the unexpected stimulus as required. 
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Advantages of state table programming State table programming provides an at- 
tractive basis for a strategic control module. 

First, it strongly encourages modular program design. Each of the state transition 
routines defined above perform a single, well-defined function. Multiple pending conditions 
are easy to handle; in fact, they are encouraged by the stimulus list paradigm. The condition 
testing “helper” routines are also encouraged to be independent modules, with a single 
purpose. Note that even complicated tests may be performed at a slower rate as background 
tasks, since they can execute asynchronously. Exception handling is also simplified, as 
exception procedures can easily be specified on a state-by-state basis. 

Second, the state table method provides an intuitive programming environment. State 
transition graphs are a very natural means of describing a task. Since they also provide 
a quick, visual overview of the entire program, they are easy to understand and modify. 
Translating the graphs to the state table format is also simple; the table format makes 
changes simple as well. 

Third, synchronization of multiple manipulator actions, or of any concurrent events, is 
natural; all events are treated as asynchronous, and are handled within the framework of 
the implementation. Both polled conditions and transient events can be handled easily. 
Conditions that are more easily tested periodically can be polled by a simple “helper” 
process that sends a stimulus when the synchronizing event occurs. Conditions that are 
more clearly modeled as discrete events can be handled by defining synchronization states. 
An example of both types of synchronization is depicted in Figure 2.4. In both cases, 
the state table method provides the programmer with a natural means of specifying the 
relationship. 



Finally, the stimulus event model directly supports a high-level request / response type 
interface. This has proven to be a very attractive paradigm for conceptual-level command 
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interaction. The user interface section discusses this in detail. 

Disadvantages of state table programming State table programming does have costs, 
mostly because it naturally produces multiple-process programs. Understanding and debug- 
ging a multiple-thread-of-execution program requires some exposure to operating system 
and concurrent programming principles. This is unfortunate, but in some sense unavoidable; 
a complex robotic system is a naturally concurrent system. Application of the hard-learned 
lessons of Computer Science is entirely appropriate. 

Also, events from various sources arise asynchronously, and must be woven into a syn- 
chronous stream of state transitions. Fortunately, modern message-passing real-time kernels 
provide exactly this service; if the stimuli are sent as messages to a state machine driver 
process, the message queuing facility performs the synchronization. 

2.5.4 Implementation 

This section describes the strategic controller implementation for our cooperating manipu- 
lator system. 


Software Structure The strategic controller structure is presented as Figure 2.5. The 
heart of the system is the Finite State Machine (FSM) driver. The FSM driver is tin 
independently executing process. It acts as a central command post, receiving messages 
from many sources in the system and taking the appropriate action. The user interface 
daemon sends command stimuli from the user interface. The arms daemon may report 
conditions detected by the arm dynamic controllers. The various helpers exist solely to 
report specific conditions when they occur 7 . Two helpers are shown: the time helper simply 
reports time-out events, and the range helper checks for unexpected object positions. 

Each message contains a stimulus code; the FSM driver uses that code to reference the 
state table, and select a state transition routine to execute. The state transition routines 
perform the actual work: they change controller modes, start and stop helper processes, 
and interact with the trajectory generation module. 

State transition graph Figure 2.6 depicts an overview of the state transition graph 
of the cooperative arms system. There are three states shown: Idle, Manipulating, and 
Inserted. These three are the only states that the system can remain in for an indefinite 
amount of time 8 . “Idle” corresponds to the arms at rest under independent control. When 
the arms are cooperatively moving an object in free space, the system is in the “Manipu- 
lating” state. Finally, “Inserted” state corresponds to the arms holding the object with a 
connector inserted into some other object, either just after completing the connection, or 
just prior to disconnection. 

Seven major tasks are supported. They are: 


7 Helpers are transient; they are only active when needed. 
8 Under normal operation. 
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• Acquire performs the object capture 

• Swap exchanges the arm grip positions 

• Insert performs a connector insertion 

• Withdraw unlatches the connector 

• Throw releases the object on a controlled trajectory 

• Home releases the object and returns the arms to their idle positions 

• Trajectory supports point-to-point cooperative motions. 

All these except “Trajectory” require a “chain” of state transitions; they are multi-step 
operations. 

2.5.5 Future Additions 

The addition of callable state sub-chains would add considerable power to the state pro- 
gramming implementation presented here. The implementation above actually uses several 
sub-chain “calls”; for instance, the insert chain calls the “swap” sub-chain if the grip must 
be re-adjusted. These axe implemented in the current version as simple “test and branch” 
code segments. A more general facility would greatly facilitate sub-chain usage. 
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2.6 User Interface 

During the last report period, functionality of the graphical user interface was expanded 
considerably. The user is now able to direct many system activities at a conceptual level. 
For unexpected or unusual tasks, manual manipulation modes are still fully supported. 

2.6.1 The User Interface as a Module in the Larger System 

The purpose of the user interface is to gather conceptual commands from the user, and 
communicate them to the strategic control module. 

The most important function of the user interface is to present a clear and intuitive 
means for the user to specify his wishes. Simple or often repeated motions should be 
automated, or at least reduced to a few short steps. The progress of the system in executing 
those commands should also be displayed in a simple, clear manner. 

The user interface and the strategic controller are quite inter-dependent; design philoso- 
phies adopted in one directly effect the effectiveness of the other. For instance, the set of 
commands supported by the strategic controller should be designed to provide a powerful 
set of primitives to accomplish most of what needs to be done. The user interface then 
need only provide a simple means of specifying those command sequences. 

2.6.2 Tele-operation versus Autonomous Control 

Tele-operation and “autonomous” operation are the two main approaches to robotic mar 
nipulation direction in the literature. In this section, we compare and contrast these ap- 
proaches, and discuss their merits. 

Tele-operation Tele-operation is defined here as control with the operator “in the loop”. 
The operator is responsible for directly controlling the system activities at all times. Tele- 
operation has two main advantages. First, since the system is under direct user control, 
it is strategically capable of performing any operation that it can physically execute. This 
makes tele-operation very flexible. All the reasoning abilities of the human operator are 
immediately available. Second, the sensor and sensor processing systems can usually be 
quite simple. The human user is amazingly adept at gleaning detailed system information 
from whatever sensors are easily available. 

Unfortunately, tele-operation also has several disadvantages. Since the user is in direct 
control of each operation, it is difficult to get assistance from the computer. Even simple, 
repetitive operations must be done manually. Precision work is difficult, or not possible. 
Operators must usually train extensively, and then devote their complete attention to the 
system operation. Finally, since the operator has complete system control, it is difficult to 
build safe-guards into the system operation. Operator error is difficult to guard against. 

Autonomous operation Autonomy is difficult to define. Perhaps the best definition 
is by the commands it recognizes; a system is more autonomous if it can accept higher- 
level conceptual commands with little supervision. It is less autonomous if it must be 
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constantly directed. Thus, we can characterize a system’s degree of autonomy by the types 
of commands it requires, and how often it requires new user input. 

The advantages of higher degrees of autonomy are obvious: the user is relieved of the 
mundane decisions, and can concentrate on more complex strategic issues. Non-expert users 
are able to utilize at least the simplest operational capabilities of the system. Also, since 
much of the system capability is specified before its actual use, the experience and care 
of the system designer can be used to program many actions. Thus, precise or dangerous 
operations can be directly assisted by the computer control system. 

Disadvantages of high degrees of autonomy are also clear. Foremost, complex au- 
tonomous actions require a level of artificial intelligence (AI) proficiency that is not cur- 
rently available. Alternatively, some autonomous actions can be pre-programmed. This is 
quite effective for simple tasks, but rapidly becomes overwhelming when the tasks become 
complex. It is also impossible to pre- conceive of all possible combinations of tasks. 

Our approach We have tried, in this research, to take a centrist approach. The simple, 
repetitive tasks required to effect an assembly — acquiring a part, fastening a connector, 
etc. — are pre-programmed, and therefore quite autonomous. More complex motions can 
then be specified as sequences of these actions. In recognition of the need for unanticipatable 
actions, a manual operation mode is also provided. This combination should allow the 
completion of most assembly tasks. 

Note that no attempt has been made at artificially intelligent assistance. This is inten- 
tional; we are attempting to create an environment where the user is tapped only for his 
reasoning and analysis abilities. This allows the study of the division of labor between pre- 
programmed primitives and reasoned actions. It is our contention (and hope) that this will 
also provide insight into a natural interface for AI interaction in the future, as a (perhaps 
gradual) replacement for the user’s reasoning capacities. 

2.6.3 Interface to the Real-time System 

The user interface must communicate with the vision system and the strategic controller. 
However, these modules execute on different processors. The graphical user interface exe- 
cutes on the Sun workstation, while the others reside on one of the real-time processors. 

A special real-time process, the “user daemon”, exists solely to arbitrate communi- 
cation requests between the real-time modules and the user interface. The user daemon 
communicates with the user interface over a “channel” (see section 2.3.4) via the VME bus 
shared memory. Although in reality all communications pass through the user daemon, 
this transaction is transparent to the underlying modules. The remainder of this section 
will describe the communications as if they were direct. 

2. 6.3.1 Interface to the vision system 

The user interface interaction with the vision system is very simple. Whenever the user 
interface is “idle”, it sends a request to the vision system for a screen update. The vision 
system then simply sends a list of the status of all the objects currently being tracked. These 
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positions are used to update the user screen display. Thus, the user is always presented 
with a current representation of the manipulator’s environment. 

2. 6.3. 2 Interface to the Strategic Controller 

This section presents the communications protocol between the user interface and strategic 
controller. 

The request / response model The nature of the protocol is a request/response pat- 
tern; the user interface requests an action, and the strategic controller returns a status re- 
sponse when the action completes. This very simple (and obvious) communication paradigm 
allows considerable flexibility. 

The design of the strategic control module directly supports this model. User requests 
are simply passed directly to the finite state machine driver in the same manner as any 
other system stimulus. Asynchronous requests do not pose a special case, since all stimuli 
are assumed to be asynchronous. Typically, each stimulus causes one “state chain” (see 
section 2.5.4) to execute. The responses to the user interface are the responsibility of the 
state transition routines that exit the chain. Since there are usually only a few of these — 
one for a successful exit, and one for each type of error exit — the status reporting code is 
very simple. 


An example: acquire A typical example of a request cycle is presented by the “Acquire” 
task. To initiate an acquire operation, the user simply “clicks on” one of the objects 
displayed on his screen. This action sends the name of the object, along with an acquire 
stimulus, to the strategic controller. Consequentially, the acquire task chain is executed. 
When the acquire operation is complete, an “object acquired” status message is returned. 
If the stimulus is not recognized by the current system state, or an error occurs during the 
operation, an “acquire failed” status is returned. 

2.6.4 Modes of Operation 

The user interface supports two modes of operation: automatic mode and manual mode. 
The user can select either mode by clicking on a button displayed on the screen. The modes 
effect only the manipulation functions of the system; the acquire and release functions are 
identical. 

Automatic mode The purpose of automatic mode is to facilitate the most common 
operations. When automatic mode is active, two “views” of the object being manipulated 
are displayed. The actual position of the object is displayed, as usual, by a solid-lined 
figure. In addition, a “desired” position of the object is drawn with dashed lines. The user 
can move the broken-line (ghost) object by clicking on it and dragging it around the screen. 
A typical portion of a user display is shown in Figure 2.7. 
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At all times, the ghost object represents the state that the system will attempt to 
produce if the mouse button is released. It is thus a one-move preview of the new state of 
the system. 

The user interface also helps the operator maneuver the object. For instance, if a male 
connector on the ghost object is moved near to a female connector on another object, the 
ghost object “snaps” to the connected position. To perform a connection operation, the 
user can simply point to any connector on the object, and drag it to the matching connector 
in the workspace. The display will show the ghost object in the final connected position. 
When the move is confirmed, the system performs the sequence of actions required to make 
the connection, and reports the status back to the user. This allows quick, simple assembly 
operations. 

Manual mode It is not possible to anticipate all possible actions a user may want to 
perform. To allow completion of actions not anticipated, a manual “tele-operated” mode 
is available. In manual mode, no ghost object is displayed. Instead, manual mode allows 
direct access to the object impedance controller. When an object is being manipulated, 
the zero point of the impedance “virtual spring” is displayed. The user can cause motion 
to occur by simply dragging the zero point around the screen. An example screen display 
is presented as figure 2.8. 

Previewing (ghost display) is disabled during manual operation; each operator motion 
is transmitted immediately to the real-time system. The object will thus follow the user’s 
mouse commands in real-time. 

Facilities are also provided to move the object’s effective center of mass and change the 
impedance gains 9 . This permits the user to exercise direct control of the system dynamic 


9 See section 2.7 for details. 
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response. 


2.6.5 A brief operational description 

This section presents a brief description of the user interface operation, and steps through 
an example application. 


A typical screen display A typical screen display is presented as Figure 2.9. The screen 
is divided into three sections. The large lower section depicts the manipulator workspace in 
iconic form; the activities of the system are visually displayed here. The upper left window 
displays the system status. The first line in this window gives a short verbal description of 
the systems activity. A system control panel forms the upper right section. 

In the example shown, there are three objects in the vision system’s field of view. 
Scooter is the floating air-cushion object. Scooter has two gripper ports, and two male 
connectors. Multibase and Dock are both stationary objects with female connectors. The 
arms are currently holding Scooter, as evidenced by the presence of the Scooter ghost 
image. The user has just dragged the ghost’s right connector over to Multibase’s rightmost 
connector. The status line indicates that the insertion of one of Scooter’s connectors into 
one of Multibase’s connectors is in progress. 


An example task: install a part An example of a typical task sequence is depicted in 
Figures 2.10 through 2.11. For the sake of this example, suppose that “Multibase” is affixed 
to a mobile robot, and represents a series of attach points for holding miscellaneous items. 
“Scooter” is a part that is to be installed into a remote module, represented by “Dock”. 
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Figure 2.9: Example User Interface Screen Display 


In Figure 2.10, the part is approaching the robot system 10 . The operator has just 
indicated Scooter is to be grasped, thus the “Acquiring Scooter” status message in the top 
left corner. In Figure 2.11, the operator indicates that Scooter should be affixed to the 
robot’s base. Next, the robot is directed to navigate to the vicinity of Dock (navigational 
control is not discussed here). When Dock is in view, the operator indicates that the new 
part (Scooter) is to be installed. Figure 2.12 shows the first stage of that action. Finally, 
in Figure 2.13, the part has been released, and the installation is complete. 

This entire procedure was accomplished with only four simple mouse motions. (Click on 
the approaching Scooter, connect Scooter to Multibase, connect Scooter to Dock, release.) 
Most of the assembly details — such as how to attach and detach connectors, how fast to 
approach the docking connector, etc. — are completely automated. 


ORIGINAL PAGE 33 
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10 Or, equivalently, the robot is approaching the part. 
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Figure 2.10: Installation Demonstration Example: 1 
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Figure 2.11: Installation Demonstration Example: 2 
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Figure 2.12: Installation Demonstration Example: 3 


! tiL.Hr Interface 1.0 - Stan Schneider I 

Idle^ 

Scale: T300] 100 1 1000 

[ Quit ] [ Help [Release] [ Hoee ) 
[ Swap ] f Throe ] Yelocl^ = 0.15^ 

Scooter: -0.201 -0.871 10.763 

Multi base: 0.141 -0.589 1.571 
Dock: -0.213 0.163 1.830 

wmm 

Mode: RCC snap: Shortcuts: Haees: 

Qnanual 0 of f Qoff Qhlde 

B auto B on B on B shoe 




J 


Figure 2.13: Installation Demonstration Example: 4 
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2.7 Dynamic Control 

Many manipulative tasks are performed more easily with cooperative manipulators. For 
example, single manipulator arms axe incapable of manipulating large objects without ex- 
erting large local torques. Precise manipulation of extended objects is difficult without 
utilizing the large mechanical advantage offered by separating the grasp points. Multiple 
cooperating arms do not suffer from these limitations. 

Unfortunately, multiply-armed robotic systems are considerably more complex. The 
dynamic equations of motion of the closed-chain system are considerably less tractable. 
Strategic control of the system interactions is much more difficult; a consistent set of desired 
motions must be specified. As of yet, no satisfactory method of precise dynamic control 
coupled with a simple strategic command interface has been developed and experimentally 
demonstrated. 

This section outlines a strategy for the control of a cooperative robotic system that 
permits high performance dynamic motion control, while also allowing direct control of 
environmental interactions. This is accomplished by controlling the manipulated object to 
react to external environmental stimuli with a programmable impedance. This facilitates 
motion direction by presenting a simple yet powerful interface; the strategic controller need 
only specify the impedance. Although “exact” inertial force compensation is achieved, 
the control structure does not require explicit formulation of the closed-chain dynamic 
equations of motion, and is amenable to parallel computation. Object internal forces are 
explicitly controlled. The object impedance controller has been implemented on a multi- 
processor real-time computer system. Experimental results for a dual two-link arm robotic 
system are presented to verify the controller’s performance, both for free-motion slews and 
environmental contact. 
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2.7.1 Background, Philosophy, and Literature Review 

This section discusses our philosophy of cooperative manipulation. We analyze the cooper- 
ative manipulation control problem, and point out the desirable features of a cooperative 
controller. Finally, we review the recent results and proposals in the literature. 

2. 7. 1.1 The dynamic controller as a module in the larger system 

In a complex robotic system, the dynamic controller is an interface between “mid-level” 
commands (such as “slew to 2.3, 4.2”) and the system’s sensors and actuators. As such, 
the salient features of an effective controller are its dynamic performance capabilities, and 
the language set of “mid-level” commands it presents to the strategic control module. Of 
course, there are several ease of implementation issues of importance as well. 

2. 7. 1.2 Control policies vs. control implementations 

The distinction is made here between dynamic control policies and control implementations. 
We define a control policy as the interface the dynamic controller presents to the next higher 
level, the aforementioned “mid-level” instruction set. The policy determines the specifiable 
ideal behavior of the system. The control implementation, on the other hand, is the method 
by which controller actually causes the specified behavior to occur. Thus, for example, 
a perfect implementation of a position control policy would maintain a desired position 
regardless of external forces. While this division is somewhat artificial, it does provide a 
framework to discuss the various techniques being used and proposed in the literature. It 
also will lead us to several new approaches to the problem. 

2. 7. 1.3 Policy objectives 

A controller’s policy determines its method of directing motion. An effective policy for 
specifying robotic motion should — at a minimum — provide for the specification of trajec- 
tories for the manipulated object to follow. It should also allow for the specification of 
the forces of interaction between the manipulated object and its environment. Lastly, and 
most importantly, it should present a simple, intuitive interface to the strategic controller. 
The instruction set should be powerful, yet easy to specify. The resulting interface will 
then allow simple specification of the actions required to perform most tasks demanded of 
a robotic system: transport, assembly, etc. 

2. 7. 1.4 Implementation objectives 

The controller’s implementation is the means by which its policy is enforced. Several as- 
pects of an implementation determine its efficacy. First, to permit accurate trajectory 
tracking performance, a control implementation should compensate for the inertia forces 
of the manipulator/object system. Second, an implementation should resolve the natural 
redundancy of the system in a beneficial manner. Two types of redundancy are present; 
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the same object behavior may be maintainable with different internal forces (static redun- 
dancy), and in multiple manipulator positions (kinematic redundancy). Third, a controller 
that interacts with its environment should incorporate force sensing, to more accurately 
control the forces of interaction. Fourth, a controller that is amenable to a parallel pro- 
cessing approach allows better performance with less expensive computational resources. 
Finally, an implementation should be verified experimentally. 


2. 7.1. 5 A review of approaches 

This section describes the various approaches to cooperative manipulation taken in previ- 
ous work, as well as proposing several new approaches. The merits of both the policies 
and implementations of each approach axe discussed. We note here that our system does 
not possess kinematic redundancy; thus we do not consider this type of redundancy resolu- 
tion. We note also that several of these approaches were implemented on our experimental 
manipulators; the experimental results section discusses their performance. 


Single arm (endpoint) control policies Currently, there are three major policies in use 
in traditional robotics — position control, hybrid position/force control, and impedance con- 
trol. Simple position control policies are useful for transport problems, but often inadequate 
for assembly tasks. Adding force control increases environmental interaction capabilities. 
Hybrid control strategies [29] permit simultaneous intermixed force and position control 
on orthogonal axes. 

Impedance control differs from these policies in that instead of controlling one state 
variable — position, velocity or force — it enforces a relation between them. This type of con- 
trol has many desirable attributes; Neville Hogan’s definitive three-part paper [16] presents 
them in detail. Chief among them are the ability to come into contact with a hard surface 
without losing stability, and the ability to specify directly the behavior of mechanical inter- 
actions with the environment. Impedance control is thus ideal for tasks requiring assembly 
or other contact with external systems. 


Multiple arm control policies Multiple arm control policies can be further divided 
into three categories: coordinated motion, master/slave, and object motion policies. We 
define any algorithm that controls the arms independently — but possibly on coordinated 
trajectories — as coordinated arm control. The arms are “coordinated” rather than “coop- 
erative” because they respond neither to each other’s actions nor inputs except through 
the object’s dynamics. Master/slave policies distinguish the arms, designating one the 
“master” and the other the “slave”. Object motion policies specify the behavior of the 
manipulated object, the manipulators themselves are abstract motion generators. Each of 
the endpoint control strategies can be extended to the multiple arm case in two manners: 
as a coordinated motion strategy, or as an object motion strategy. 
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Coordinated motion approaches Each of the endpoint control strategies can be ex- 
tended to the multiple arm case as a coordinated motion policy. Unfortunately, no coordi- 
nated motion policy presents an attractive cooperative control strategic interface. It is also 
difficult to compensate for the full system dynamics with a coordinated motion strategy. 

Coordinated position control is inadequate for even simple object positioning tasks; since 
the closed-chain cooperative system is kinematically over- constrained, any minor endpoint 
positioning error will cause the build-up of large internal forces. Zheng and Luh present a 
coordinated motion approach [43], along with some preliminary experimental results. They 
solve the kinematic over-constraint problem by manipulating a deformable object; they do 
not consider inertial forces or environmental interactions. 

Mason [26] developed constraint relations for multiple-armed coordinated hybrid con- 
trol. His development solves the kinematic over-constraint problem, but presents a compli- 
cated strategic interface. The arms must never be allowed to conflict in a single degree of 
freedom — either both in position control mode or both in force control mode. Extension of 
this requirement to more than two arms is completely unwieldy. His work does not consider 
dynamic interactions, and no simulation or experimental results are given. 

Coordinated impedance control [28] presents a simpler interface, and is capable of good 
performance in many tasks. Unfortunately, while the arm dynamic properties are easily 
taken into account, it is difficult to compensate for the object dynamics with this control 
scheme. It is also complex to specify external interaction forces. 

Master/slave approaches Much of the pioneering work in cooperation utilized some 
variation of master/slave control [17, 12]. More recently, Alford and Belyeu [3] modified 
the slave’s trajectory in real-time to minimize the error in the positions of the end effectors. 
These techniques do not consider environmental interactions. They also suffer from the 
artificially imposed asymmetry, and none take into account the dynamic behavior of the 
system. Moreover, from a strategic viewpoint, master/slave control is a difficult policy to 
direct. Free-space motions are the only simply specifiable actions. 

Object motion approaches Object control policies specify the object’s behavior — no 
independent arm action is specified. Each of the endpoint control policies can also be 
extended to the multiple arm case as an object control policy. Object control policies 
have the distinct advantage of providing the strategic controller with the ability to specify 
the object motion without regard to the manipulator details. Object control also raises 
some difficult implementation issues — no previously published experimental results consider 
inertial force compensation. 

Several researchers propose object position control algorithms. Tarn, Bejczy, and Yun 
[37] developed the closed-chain dynamic equations of motion, and utilized nonlinear de- 
coupling feedback to control the object position. This treatment compensates for the full 
system dynamics. However, it completely ignores the effect of external forces. Also, since it 
utilizes the closed-chain equations of motion, it must be implemented serially. Nakamura, 
Nagai, and Yoshikawa [27] develop a multi-fingered hand control scheme that specifies the 
dynamic behavior of the manipulated object. Their formulation also allows specification 
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of the object internal forces. They also propose a parallel dynamical compensation scheme 
based on D’Alembert’s principle. However, they too assume the object is not in contact 
with its environment. No simulated or experimental verification is offered. 

Hybrid object force/position control permits interactions with the external environment. 
Uchiyama, Iasawa, and Hakomori [39] present an experimental hybrid controller using 
two arms to carry an object under compression. They define an interesting workspace 
coordinate system that allows hybrid control of both external and internal positions or 
forces. Their algorithm is based on static analysis; it is incapable of dynamic compensation. 
The experimental task they present is a simple transport problem, with no environmental 
interactions. 

Hayati [13] presented a theoretical extension of hybrid force/position control to the 
multiple manipulator case. He defines an arbitrary task reference frame, a useful concept 
incorporated into the controller developed in this paper. Although a parallel implementa- 
tion is proposed, his controller requires the closed-chain equations of motion and is compu- 
tationally complex. It also specifies locally closed force loops on each arm; in practice, this 
can make the control very sensitive to force offset errors. 

Oussama Khatib, in [20], extends his operational space control methodology to incorpo- 
rate multiple manipulators. His paper’s main thesis is a proposed dynamic implementation 
technique based on summing the object inertia and the arms’ effective tip inertias. The 
formulation allows utilizing operational space hybrid force/position control. The algorithm 
takes into consideration the full system dynamic response, but is not particularly well suited 
to parallel implementation. Internal forces are not controlled; the redundancy is used in- 
stead to minimize the actuator loads amongst the manipulators. This approach has not yet 
been experimentally verified. 

Hybrid control does not, in the authors’ opinion, provide an attractive strategic-level 
interface. Since separate force and position controlled subspaces must be maintained, con- 
trol mode switching decisions must be made at many points during most tasks. Specifying 
sequences of these mode switches is often not intuitive. Unexpected situations make these 
decisions even more difficult; the set of ’’natural” constraints may not be easily deter- 
minable. 

Impedance control provides compliant response at all times. Selection of significantly 
differing stiffnesses in different directions must be done only when required by the situa- 
tion. The more general impedance compliant response also eases interactions with moving 
environments. 11 

Object impedance control is the subject of the next section. It provides a simple, power- 
ful interface for directing motion and environmental interaction. Allowing the specification 
of an arbitrary task frame and effective object dynamic response permits intuitive and gen- 
eral task sequence specification. A parallel implementation that compensates fully for the 
system dynamics, and directly controls the object internal forces is presented below. 


11 This is especially important if the “environment” is an object being controlled by another robotic 
system — we are also concerned with extension to multiple teams of robots. 
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2.7.2 Object Impedance Controller Derivation 

This section restates the motivation for object impedance control, then derives a dynamic 
implementation . 

2. 7.2.1 Control objective 

Hogan’s impedance control policy, described above, causes the endpoint of the manipulator 
to react to external forces with a programmable impedance. The simplest example both 
to understand and implement is a simple second order linear impedance — the endpoint 
behaves as a mass attached via a virtual spring-damper to the environment. In this section, 
we develop a controller that enforces a controlled impedance not of the arm endpoints , but 
of the manipulated object itself. Intuitively, the object behaves as if it were attached to its 
environment by linear spring-damper systems in the linear degrees of freedom, and also by 
uncoupled torsional spring-dampers to control rotational orientation. 

Policy The object impedance controller enforces (for a simple linear second-order im- 
pedance) the relationship: 

m d (x - x dea ) + k v (x - x dea ) + k p (x - x dea ) = f ext (2.1) 

Here x denotes the coordinate of any one degree of freedom of an arbitrary point of the 
object. The constants m d , k v , and k P are specifiable. The reference signal x dea denotes the 
desired position (or orientation) of the chosen point. The x dea term represents acceleration 
feed-forward. Thus, the programmable impedance force corrects deviations from the desired 
trajectory. 

Intuitively, this control policy completely supplants the actual dynamics of the object 
with a “virtual” object, with specifiable mass and inertia properties. The “virtual” object 
is attached at its (apparent) center of mass via an orthogonal set of imaginary damped 
springs to a selectable point in the environment. Thus, the object can be manipulated by 
simply moving the virtual spring endpoint. Controlled force interactions with environmental 
obstacles can be done by simply pressing the “spring” against the obstacle. 

We note here that a more general impedance can be modeled as: 


m d X — fext + /imp 

Here /, mp is an imaginary impedance “force”, it is usually a function of only position and 
velocity, but can be quite arbitrary. In particular, the non-linear “potential field” forces used 
in many obstacle avoidance techniques [22] can be easily incorporated by simple summation 
(for proof, see [16]). 

This controller accomplishes all the policy objectives outlined above. The strategic 
interface is quite simple and straight-forward. The strategic module directly selects the 
object’s behavior — the arms are very much an abstract manipulation system. In addition, 
to the bandwidth of the control system, the object dynamical properties are also selectable. 
Critical damping can be enforced in all degrees of freedom simultaneously. Contact of 
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the manipulated object with the environment is not a special case requiring control mode 
switching; the impedance controller handles it in a natural manner. Thus, both free motion 
slews and manipulation requiring contact can be done with the same strategic interface. 

The position and orientation of the “virtual” object with respect to the actual object is 
also selectable. Thus, this controller is also capable of pseudo remote center of compliance 
(RCC) operation [41], permitting simple and efficient part mating and insertion operations. 
A particularly useful example is for performing connector insertions — by placing the “vir- 
tual” object frame at a fixed location in the connector frame, all assembly operations can be 
specified as connector motions only. Multiple connectors arranged on an object in arbitrary 
orientations can then be handled by the same simple connector insertion algorithm. 


Implementation Implementation of this type of controller requires consideration of the 
dynamics of the entire system: both arms and manipulated object. Utilization of the 
full system equations of motion is one method of performing this control, but we have 
developed an implementation based on Nakamura’s multi-lingered hand controller [27] that 
isolates the system into three sub-systems: arml, arm2, and the common object. This 
vastly simplifies the computations required at runtime; the closed-chain equations of motion 
are not required, and the algorithm can easily be implemented by a parallel processor 
architecture. In addition, the internal force in the object is directly specifiable. 

Force information at the arm endpoints enhances performance on two counts: better 
control of the internal forces and more accurate measurement of the forces of interaction 
between the object and its environment. 


2. 7.2. 2 Derivation 


The derivation presented below is rather straight-forward: The desired object acceleration 
is found, and substituted into the actual object equations of motion. The resulting accel- 
erations and forces at the arm endpoints are found, and then implemented on the arms as 
a “computed-torque” (CT) control law. 
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Notation The symbols used axe: < 


Symbol 

Type 

Description 

n k 

3x1 

inertial frame unit vector 

bk 

3 x 1 

object body frame unit vectors 

m 

scalar 

mass of the manipulated object 

M 

3x3 

m times the identity matrix 

I 

3x3 

inertia matrix of the object relative to its center of mass for {bj} 

f ext 

3x1 

external force on the object (not due to the arms) 

T ext 

3X1 

external torque on the object (not due to the arms) 

X 

3 x 1 

inertial frame position of the object center of mass 

y 

3x1 

inertial frame position of the apparent center of mass 

T 

3x1 

y — x (offset of apparent from actual center of mass) 

Tk 

3x1 

rn k 

U> 

3 x 1 

object angular velocity 

g 

3x1 

gravity vector 

u n 

n x n 

identity matrix 

N 

scalar 

total number of arms contacting object 

a, 

3x1 

inertial frame position of i th arm’s endpoint 

Pi 

3x1 

body frame position of i th arm’s endpoint 

Pik 

scalar 

Pi "k 

fi 

3x1 

force on object due to i th arm’s endpoint 

m, 

3 x 1 

torque (moment) due to i th arm’s endpoint 

q< 

HUnks X 1 

vector of i th arm’s joint angles 

T, 

flltnks X 1 

vector of i th arm’s joint torques 

f«'mp 

3 x 1 

imaginary impedance “force” 

T imp 

3 X 1 

imaginary impedance “torque” 


Actual object dynamics The object’s free-body diagram is shown in Figure 2.14. The 
object’s center of mass is offset in the inertial frame by x. We assume there are N total 
manipulators in contact with the object, two are shown. Arm i exerts a force f t - and moment 
m< at a point offset by the vector p t - from the center of mass. A second arm, j, acts in a 
similar manner. External forces and torques act on the object — they are modeled by their 
resultant force f ex t and torque r ext acting at the object’s center of mass. 

The object’s equations of motion are: 

mx = i ext + fi + mg 


/W+«X/W = Text + ^2 Pi X f, + ^2 m, 

Using y = x + w x r + « x (w x r), the equations of motion can be expressed in matrix 
form as: 


mUj 

mR 

y 

+ 

-mg - m(u> x (a? x r)) 


^ext 

_L 


0 

I 

U) 

U) x Iu> 

- 


T ext 

T 

£ Pt x fj + £ m, 
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Figure 2.14: Object Free-Body Diagram 




or, more compactly: 
where 


I 0 Y + B 0 = F ext + WF 


Y 


and 



W = 


U 3 0 U 3 0 
Pi U 3 P 2 U 3 


U 3 0 
Pn U 3 ’ 



0 

-r 3 

r 2 


0 

~Pi 3 

Pi2 

r 3 

0 

-r i 

,Pi = 

Pi 3 

0 

~Pi 1 

. _r 2 

** l 

0 


~Pi2 

Pi 1 

0 


( 2 . 2 ) 


Desired object behavior The desired object behavior is depicted in Figure 2.15. The 
same external forces and r ext act on the object. Point C v is the arbitrarily chosen 
apparent center of mass. It is offset from the center of mass by r, and in the inertial 
frame by y. The imaginary impedance “force” £ mp and “torque” r, mp act at point C v . 
The apparent object mass and inertia are also specifiable as m d and I d . Thus, the desired 
equations of motion are: 


M d y — f ext 4" f imp 


I d (j + U> X I d U> - T ext + T irnp - r X f el t 



36 


CHAPTER 2. FIXED-BASE COOPERATIVE MANIPULATION EXPERIMENT 



Figure 2.15: Desired Object Behavior 


The desired behavior in matrix form is: 

+ 


Mi 0 

0 h 


y 

Cj 


0 

U> X I d UJ 


lext 

T ext f X fext 


+ 


hmp 
r imp 


LodY + Bod = F e;nt + F ; 


tmp 


(2.3) 


Controller Derivation So, from 2.3, the desired acceleration is: 

Ycmd = ^Qi [F ext "b F ,' m p — Bod] 

This requires (using 2.2): 

WF = Bo - F ex t + /<,(# [Fext + F, mp - Bod]) (2.4) 

To choose the commanded forces, F cm d, let 

F cm d = ^{Bo - F ext + Io(I; d l [F ext + Fimp - Bod])} + t internal (2.5) 

where W wpt is a weighted pseudoinverse of W, and f 'internal is any vector in the null space 
of W. The section A note on load balancing below details how these may be chosen. 

We now have f,- for each arm. To calculate the desired arm tip accelerations a,-, use: 


£. cmd = x+«Xpi+Mx(«xp l ) 

Then, letting Af, and 7,- denote the individual arm mass matrix and Jacobian, use the arm 
kinematics: 


SUcmd — J i 1 [at C m d ~ 



2.7. DYNAMIC CONTROL 


37 


and the arm equations of motion: 

Ti = M,q iemd + C(qi,qi) + Ji T fi emd 
to calculate the desired arm torques. 

External force calculation We have yet to estimate F ex <. We can use equation 2.2, 
but we first need to estimate Y. We tried two approaches: 

using the last commanded acceleration: Y — 
and using the desired acceleration: Y = Ydes 

Each of these yielded acceptable experimental results. In practice, the accelerations are 
small when the external forces axe significant. Since the acceleration estimate is only used 
to estimate the external force, it is only critical when the object is in contact with an 
accelerating environment. This is a rare case, but may be useful, for example, to insert a 
battery pack into a rotating satellite. We are investigating more sophisticated acceleration 
estimation techniques. 

A note on load balancing Equation 2.5 has two terms. The first term, W w1in { • • •}, 
represents a particular solution to equation 2.4. This vector of forces produces the desired 
motion while countering the inertial, gravitational, and external forces. The second term, 
^internal i is any member of the null space of W, the subspace of forces that produce only 
internal loading on the object. Normally, internal can be chosen simply to provide the 
desired internal object forces when the object is at rest. 

Some latitude exists in choosing W wp ', the weighted right pseudoinverse: 

W wpi = q- 1 w t (wq- 1 w t )~ 1 

Where Q is a diagonal matrix of task-space weights. If Q is the identity matrix, then the 
solution will be the minimum norm vector of endpoint forces and torques — all degrees of 
freedom will receive equal preference for loading. Since manipulators are usually capable 
of generating considerably more force than torque at their endpoints, Q will usually be 
used to weight the linear degrees of freedom more heavily. Task-space load distribution 
(in a static environment) is studied more completely in [1]. A comparison with joint space 
optimization techniques can be found in [36]. 

Controller structure The overall controller structure is depicted in figure 2.16. Note 
that the system divides naturally into N + 1 parallel computations: one to calculate the 
object dynamics, and one for each arm’s dynamics 12 . Our experimental system employs 
three processors, configured in exactly this manner. 

12 The arm calculations could be similarly sub-divided, see chapter 3. 
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Controller block diagram Figure 2.17 depicts the resulting controller block diagram. 
First, measured and desired object position and velocity are used to calculate the impedance 
“force”, and the incoming measured forces at each manipulator endpoint are used to esti- 
mate the external force on the object. The desired behavior equations then produce a new 
desired acceleration command. Simple object kinematics then provide arm endpoint accel- 
erations, and the object equations of motion are used to calculate consistent commanded 
endpoint forces. These commanded values are in turn used by each arm’s computed torque 
controller to servo the arm actuators. Each of the three major boxes share no intermediate 
values; thus they can be run in parallel. (In fact, our experimental implementation runs 
the three loops at different, asynchronous rates.) 

An intuitive alternate controller A slight variation in this derivation yields a con- 
troller with different properties. When the r is non-zero, the controller derived above 
above causes the object to behave as if its center of mass were moved to the impedance 
force action point. Thus, the object dynamic behavior is completely specifiable — the actual 
dynamics are completely canceled. This provides exact dynamic decoupling of all degrees 
of freedom, even under RCC operation. A simple change in the desired motion equations 
to: 

MdX = f ext "1" Lmp 

Id + W X IdU> = T ext + T imp + T X limp 

leaves the object’s apparent center at the actual center of mass. This controller does not 
completely decouple the object orientation from the linear motions. Although this is less 
appealing from a mathematical viewpoint, experiments with both manual operation and 
strategic programming showed that this controller is sometimes more intuitive to use. An 
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Figure 2.17: Controller Block Diagram 


example is depicted in Figure 2.18. Users expect that the object pulled by a “spring” 
attached at the end will rotate — the real world is dynamically coupled. Moving the center 
of mass decouples these motions and results in a linear translation only. The strategic 
control module permits the user (or strategic programmer) to select either controller. 

The two controllers are identical when r = 0, and they have identical static behavior, 
since static balance requires f ‘ ext = — f, mp . The coupled controller does however, have 
practical implementation advantages. It is less sensitive to biases in the force sensors (since 
f imp does not depend on measured force). It also makes better use of the available actuator 
authority, since moving the apparent center of mass often increases required (peak transient) 
manipulation forces. 
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Figure 2.18: Intuitive Alternate Controller 
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2.7.3 Experimental Results 

2. 7. 3.1 Experimental Dual Manipulator System 

The experimental system is designed to emulate a dual-armed space robotic manipulator. 
The arms axe each direct-drive, SCARA configuration, two-link manipulators. They are 
equipped with pneumatic force-sensing grippers. The grippers connect to “ports” on the 
floating object via a bearing pin joint. Thus, no torque can be delivered to the object by 
a single arm. The manipulated object floats over the granite surface plate with negligible 
friction. A picture of the experimental system is presented as Figure 2.19. 



Figure 2.19: Experimental Dual Arm Manipulator System 


2. 7. 3. 2 Transport trajectory tracking 

This section presents a comparison of the performance of three cooperative control algo- 
rithms for a cooperative transport task. The three algorithms are: co-located proportional- 
derivative joint-space coordinated position control, coordinated endpoint impedance con- 
trol, and object impedance control. The starting and ending positions for the test slew are 
indicated in Figure 2.20. Note that all the arm joints undergo large angular changes, and 
that both position and orientation are effected. The commanded reference is a fifth-order 
trajectory of the center of mass of the object in each object degree of freedom: x,y, and 
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9 [10]. The slew reference takes 1.7 seconds to complete its path — faster slews caused our 
(rather weak) motors to saturate. 



All algorithms were provided with the correct coordinated position, velocity, and accel- 
eration references for the entire slew path. The gains on all the controllers were set for fairly 
stiff operation (to yield the best trajectory tracking), and are set as “fairly” as possible. For 
example, the PD joint angle controller gains were calculated to provide the same corrective 
force at the tip as the operational space controllers would provide for a similar positioning 
error. Several other gain combinations were studied. Where appropriate, effects of this 
variations are discussed below. 

In the figures that follow, the upper-left plot depicts the motion of the center of mass 
of the y direction. The lower-left is the corresponding velocity. The upper-right plots x 
vs. j/, and indicates the desired and actual object positions at 0.5 second intervals during 
the motion. The lower-right plot shows the magnitude of the “tension” between the arms, 
after being corrected for dynamic forces. All controllers are attempting to maintain zero 
tension. Note that the force sensors exhibit some orientation-dependent bias (maximum 
magnitude is approximately 0.2 Newtons), thus some “tension” is due to sensor error and 
is unavoidable. 

Coordinated PD control Figure 2.21 presents the performance of a simple joint-space 
PD controller. Co-located PD control implements the algorithm: 

7* = ^p(*l desired Q) "I” AT v (c [ desired ~ ^l) 


for each joint. 

This controller does a poor job of following the desired trajectory and offers no control 
of the internal forces on the object. This controller also does not compensate for inertial 
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Figure 2.21: PD Controller Slew Performance 

forces. Reducing the gains caused the trajectory performance to deteriorate badly, while 
failing to eliminate the tension buildup. 

Coordinated endpoint impedance control Figure 2.22 depicts the performance of 
coordinated endpoint impedance control. This controller implements the dynamic spring 
relationship: 


m desired(P desired ~ P) 4" K v (p desired ~ P) 4" Kp{Pdesired P) — f tip 

on each arm, where p is the tip position, and f ttp is the measured force at the manipulator 
endpoint. 

Substituting p into the arm kinematic equation 

q = J -1 [p - jq] 

yields joint accelerations q. The arm equations of motion: 

r = Mq + C + J r f tip 

then yields a dynamically compensated “computed torque” impedance controller. In this 
relationship, f ttp is the measured external force on the manipulator tip. This controller thus 
incorporates force feedback to enforce the endpoint impedance relationship. 

This controller exhibits much better trajectory tracking performance (Figure 2.22). 
Although the object inertia forces are not compensated for, the feedback — coupled with 
acceleration feed- forward — is sufficient to cause very little trajectory tracking error. Unfor- 
tunately, to yield this level of performance, the impedance gains had to be fairly stiff. As a 
result, the inter-arm tension is not controlled well. Slight kinematic errors are sufficient to 
cause a large internal force at the end of the slew. Lowering the gains significantly reduced 
the tension problem, but also detracted from the trajectory accuracy. 
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Figure 2.22: Coordinated Endpoint Impedance Slew Performance 

Object impedance control Figure 2.23 displays the results of the object impedance 
controller. Since this controller correctly compensates for the object dynamics, the trajec- 
tory tracking performance is quite impressive. The inter-arm tension is controlled well also. 
Lower gains did not effect the trajectory accuracy much. 

2. 7.3. 3 Force control performance 

Figure 2.24 compares the response of the object impedance controller’s external force mea- 
surement with the expected response of an ideal impedance (equation (POLICY)). These 
data were obtained by simply placing a hard, stationary object in the path of the object 
during a slew (in the x direction). Raw versus filtered (10Hz 2-pole digital Butterworth fil- 
ter) data from a single arm (the ri" 1 '* arm) is presented in Figure 2.25. The unfiltered data 
exhibits some ringing — this is an impact between two very hard objects — but the force level 
quickly settles to the desired. The important thing to note is that the object impedance 
controller successfully controls the forces of interaction, without switching control modes, 
even when it comes into contact with a very stiff environment. 

2. 7. 3. 4 RCC operation 

Figure 2.26 shows a “strobe” picture of the object under control, with r non-zero. The point 
C v is selected at the right edge of the object. The data used to generate this plot were 
generated by pushing the object (manually) in the positive y direction, and then releasing. 
The estimated external disturbance force is depicted in Figure 2.27. The “spring” gains 
chosen for this experiment are stiffer in the linear directions than in orientation, thus the 
object rotates about the point C v with very little translation. 
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Figure 2.27: Estimated Force for Remote Center Rotation 
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2.7.4 x Conclusions 

This section has presented a dynamic control policy and implementation for multiple- armed 
manipulator systems that features: 

• A simple, powerful object behavior specification interface. 

• Flexible control specification: both for free motion positioning and environmental 
interaction. 

• Good dynamic performance, both in free motion and in contact, without switching 
controllers. 

• Exact dynamic compensation, without requiring closed-chain equations of motion. 

• A naturally parallel structure, allowing simple implementation on a multiple processor 
computer system. 

• Remote- centered compliance operation, facilitating assembly operations. 

This controller has been implemented experimentally, and proven to be an effective 
dynamic control module in an overall cooperative manipulator system design. 
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2.8 Real-time Vision System 

During this report period, a high-speed television camera-based point-tracking vision sys- 
tem was implemented. In addition, algorithms for identification and tracking of moving 
two-dimensional objects were developed, and used to demonstrate vision-guided dual-arm 
intercept and capture. This section discusses the results of that effort. 

2.8.1 The vision system as a module in the larger system 

The vision system has a well-defined function: to interpret the video data, and report infor- 
mation about the various objects in the workspace. This function is complicated somewhat 
by the varying nature of the different modules’ need for the information. For example, 
some modules — such as the user interface — may simply require position information at 
rather slow update rates. On the other hand, the task of catching a moving object requires 
not only position measurements, but also velocity estimates. Finally, control modules re- 
quire object and arm endpoint information at very high rates and with minimal delay. The 
vision system must provide the information in the proper form to each module. 

2.8.2 Vision System Structure 

The vision system software structure chart is presented as Figure 2.28. Video information 
from the CCD television camera is digitized and stored into a video frame buffer. A “UFO” 
searcher process scans the video data, looking for new points to track. When a point is 
located, it is “installed” with the point manager, which arranges for its being tracked in 
the future. 

The object managers are each responsible for managing one type of object in the system. 
For example, the “body” manager is able to identify any of the various rigid bodies in the 
view. It also can calculate the body’s position and orientation, given the location of its 
targets. Object managers can optionally employ the observer service to implement a simple 
Kalman filter, thus providing an estimate of the object’s velocity as well as its position. 

The object manager manager acts as a liaison between the rest of the vision system and 
the various object managers. 

2.8.3 Sub-pixel real-time point tracking 

Appendix A of the Fifth Semi-Annual Report presented the theoretical basis of a proposed 
sub-pixel accuracy real-time point tracking system. In summary, this system allows de- 
termining the location of special passive optical “targets”. Preliminary simulation results 
suggested that the positions could be determined to approximately l/20 (/l of a pixel accu- 
racy, at the full frame rate (60 Hz) of the camera. Our experimental findings have confirmed 
this prediction. 

The passive targets used vary in reflectivity, from black at the edges to white in the 
center. An example target is presented as Figure 2.29. They span an area of approximately 
8 by 8 pixels. Thus, each pixel in the 8x8 grid contains information about the location 
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Figure 2.28: Vision System Software Structure 


of the center. (In the noise-free case, one could calculate the distance to the center from 
each pixel exactly.) A simple, fast, centroid (center-of-brightness) calculation then yields 
an estimate of the actual center. 

The point tracking module maintains a list of known target locations. At each frame 
interrupt, the new target location is calculated. To track quickly moving points, a simple 
digital filter is used to estimate each point’s velocity. The system can then generate a better 
estimate of the target’s expected location in the next frame. Thus, points should be “lost” 
only when they experience a high acceleration 13 . 

2. 8.3.1 Viewer and the Vision Daemon 

To assist in debugging the code, a program called “viewer” was deve'^ped. It is presented 
here to assist the reader in understanding the concepts involved. The “viewer” program 
runs on the Sun workstation. It opens a channel to the vision system; the “vision daemon” 
process exists solely to interact with the viewer. Two fields of information are displayed: 
the list of active point coordinates (in pixels), and the camera field of view. The active 
points flash in the camera view, so they can be easily located. A typical screen display is 
presented as Figure 2.30 

2.8.4 “UFO” Searcher 

The function of the “UFO” searcher module is to locate and identify new points in the field 
of view. The “UFO” searcher runs as a background task, completely asynchronously from 
the rest of the system. 

13 In practice, several effects can cause the point to be “lost”. A more detailed analysis will be presented 
in the next report. 
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Figure 2.29: Example Target 


To locate new points, it must first be able to distinguish the special optical targets from 
the other clutter in the scene. The algorithm for this is rather simple. First, the scene is 
scanned, looking for brightness transitions. When a transition is found, the coordinates are 
tested to see if this is already a tracked point. Next, the point-tracking centroid algorithm 
is executed once to find the center of the candidate point. 

If the centroid algorithm returns a reasonable target center location, the following tests 
are made to determine if the point is indeed a target. First, a small “box” is drawn around 
the suspected target center. All pixels on this path (lightly shaded in Figure 2.31) should 
be above a programmable threshold value. Next, a larger box is drawn around the center. 
All pixels on this path (heavily shaded in Figure 2.31) should be below the threshold. If the 
point passes both tests, it is declared a new point, and installed with the point manager. 
This rather crude test only checks for points of a specified size, but suffices in our rather 
controlled environment. More sophisticated tests, such as matching the target reflectivity 
profile, could be easily performed if they were deemed necessary. 

Scanning the camera’s field of view by this method takes about 30 seconds. To increase 
the efficiency of the scanning process, the “UFO” searcher also accepts “hints” about where 
to look for new points. Any module may request that a Region Of Interest (ROI) be quickly 
scanned for a new point. All the ROI requests are serviced on a circular queue basis. Thus, 
each ROI is scanned for a short time; then scanning attention is turned to the next ROI. 
The entire view is always installed as one ROI to provide over-all scanning. With the 
exception of the entire view ROI, all other ROIs have a limited lifetime; they are removed 
from the circular list after a specified number of scans. 

This scheme allows modules that have a good idea of “where to look” to install a 
very small ROI, and ensure that the point(s) of interest are quickly located. For instance, 
the arm endpoint managers install a small ROI around the location of the arm endpoint 
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Viewer 2.U - Stan Schneider 


Threshold: [1271 9 

0: 271.684, 120.233^ 

1: 

221.826, 

129.786 

2: 226.367, 143.979 

3: 

236.851, 

136.812 

4: 166.388, 121.299 

5: 

145.325, 

216.724 



Figure 2.30: Viewer Screen Dump 


calculated from the joint angle measurements. This point is known relatively well, thus it 
is usually found in a fraction of a second. 

This capability is critical to the moving object acquisition problem. During a “catch” 
attempt, the object often passed beneath one of the approaching arm links. This obscures 
the targets, and causes the object to be “lost” at a critical time — just before the grippers 
are to engage. To remedy this, the body manager simply installs a ROI at the expected 
location of the object, based on its last known position and velocity. When the object 
comes back into view, it is located quickly and the grasping operation can continue. 

2.8.5 The Point Manager 

The function of the point manager is to maintain a list of tracked points, and ensure 
their positions are updated each frame interrupt. The point manager also performs the 
pixel-data-to-world-coordinates transformation, allowing all higher-level modules to work 
in natural (metric) units. 

New points are provided by the “UFO” searcher. These are maintained on a list of active 
points. When installed, each new point is “unmanaged”. Whenever the list of unmanaged 
points changes, the point manager passes it to the object manager manager for analysis 
by the various object managers. If an object manager recognizes a point, it claims the 
point (by setting its manager field). That point will then not be considered in future scene 
analysis attempts. 

The location of each point on the active list is updated each frame interrupt 14 . When 

14 Actually, the point’s object manager can request less frequent updates if the point is not expected to 
move much. This saves considerable processing time. 
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Figure 2.31: New point identification algorithm 


a point is lost for some reason, the point manager notifies the point’s object manager (if it 
has one), and removes it from the active point list. 

To transform pixel locations to world coordinates, two operations are required: the 
pixel value must be converted to meters, and the effects of parallax must be corrected. At 
this point, this transformation is very fast and simple; pixels are converted to world frame 
coordinates via a simple scale and offset calculation, and the parallax effects axe included in 
the scale. Figure 2.32 shows the parallax correction technique in one dimension. By similar 
triangles, each height above the table has a different pixel-to-meters conversion factor, given 
by the formula: 

ParallaxCorrectedScale = ScaleAtTableH eight * i Cam ^ aH ^ % 9^ ~ height) 

CameraHeight 

Since all points in our system have a fixed height, this calculation need only be done once, 
when the point is identified. 

2.8.6 The Object Managers 

There are currently three object managers in the system: the body manager, the arm end- 
point manager, and the point server. The body manager can identify and track rigid bodies 
in three degrees of freedom: translation in x and y, and rotation in 0. The arm endpoint 
manager is responsible for tracking the arm endpoints, and providing this information to 
the dynamic control module. Any module on any processor can specify a single point to 
be tracked by the point server. The point server is not used (yet) in this system (it will 
soon supplant the arm endpoint managers), but it allows the vision system to serve point 
location information to other experiments that share the vision system 15 . 

15 In particular, the two-very-flexible-link endpoint control experiment. 
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Each object manager must register with the object manager manager (OMM) at start- 
up time. The OMM then maintains a list of active object managers. Registration requires 
providing four routines: 

• an identification procedure 

• a lost point procedure 

• a frame interrupt procedure 

• and a report position procedure 

The identification procedure is passed the list of unclaimed points when the “UFO” searcher 
changes it. The lost point procedure is called wuen a point claimed by this object manager 
is lost. The frame interrupt procedure is called each frame interrupt. Finally, the report 
position procedure is called to request that the managed object’s positions be sent to the 
user interface. 

2.8.7 The arm endpoint manager 

The joint angle sensors can be used to estimate the endpoint position of each arm. Un- 
fortunately, this estimate does not exactly match the vision system estimate of the same 
location. This is due to several effects, among them are angle measurement error, kinematic 
parameter errors, and optical distortion. Since the measurements of interest to the system 
(object locations, docking connector locations, etc.) are provided by the vision system, the 
vision system, estimate of the arm endpoints must also be used by the dynamic controller. 
This endpoint control ensures that the controlled arm gripper position accurately matches 
the positions of the gripper ports on the objects of interest. It is the function of the arm 
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endpoint manager to provide the vision-sensed endpoint location to the dynamic control 
module. 

The arm endpoint manager object procedures are simple. The identification procedure 
simply scans the unclaimed point list for a point near the kinematic estimate of the arm 
endpoint. The lost point procedure installs a small ROI around the arm endpoint expected 
position, and directs the dynamic controller to use the kinematic endpoint estimate. The 
frame interrupt procedure reports the endpoint location to the dynamic controller. The 
report position procedure is currently null, it will be added when the user interface is 
expanded to allow direction of individual arm motions. 

2.8.8 The Body Manager 

The body manager is responsible for identifying and tracking all the rigid bodies in the 
system 16 . At least three target points are affixed to each body. These points are in a 
unique pattern on each different type of body. The body manager is provided a database 
describing each body at initialization time, this database includes: 

• the body’s mass and inertia 

• the location (and height) of the targets on the body 

• the location of any gripper ports on the body 

• and the location and type of any connectors on the body 

As an ancillary function, the body manager provides any needed data about the body to 
other modules in the system. For instance, the dynamic controller requires the mass of any 
object it manipulates; the body manager provides this information. 

Identification The body identification algorithm exploits the fact that the target con- 
figuration on each object is unique. When passed a list of unidentified points, the body 
manager first constructs a “UFO” point map, specifying the distances between all the 
points in the system. Figure 2.33 presents an example. This point map is then scanned for 
matches with the known distances between the targets on each object. If a sufficiently close 
match can be found for each target on an object, a quick consistency check is performed 
to ensure the identification is correct. If the matches are consistent, then the object is 
considered found. 

Position and orientation update Each frame interrupt, the position and orientation of 
the object is calculated. The algorithm is based on the fact that the “center of brightness” 
of the targets is independent of orientation. More precisely, the average of the positions of 
all the object’s targets can be calculated without regard to the object’s orientation. It can 
then be used as a basis for determining the object’s orientation. 

16 The term body in this section refers to any single rigid body whose position and orientation are both of 
interest. This includes the floating “object”, as well as the docking ports, etc. 
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Figure 2.33: Body Identification Algorithm 


The algorithm is depicted schematically in figure 2.34. During initialization, the “center 
of brightness” r cen of the object’s targets (in the object’s frame) is calculated. The vector 
to each target a,- in the body frame is also calculated. 

To calculate the object position and orientation, the vector r and the angle 9 must be 
found. When the target positions are known, the new “center of brightness” d is simply 
the average of all the coordinates. Let bj be the vector from d to the i ifi target, i estimates 
of cos(Q) are then available as: 

cos(0) = (a i • bj)/|a,j|b,j 

These estimates are combined in a weighted average to yield an estimate of cos(0). The 
optimal weights to use are 17 : 

w i = |a«| 2 /(k*| + Kyi) 

The angle 9 is then calculated via a single arc cosine subroutine call. 

This procedure will result in a 9 value in the range [0, tt). To determine the sign of 9, 
note that: 

bi x = cos(0) * a, x - sin(0) * a, y 

Since the sign of 9 is the same as the sign of sin(0), theta is positive if: 

a ty > 0, and cos (9) * a; r - &,* > 0 

These equations, along with similar ones for b{ y , yield 2 i independent estimates of the sign 
of 9. The estimates axe combined via a simple voting method: if more estimates indicate 
negative sign than positive sign, 9 is assigned the negative value. 

17 Proof will be supplied in a future report. 
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Figure 2.34: Body Position Calculation 


Once 6 has been calculated, the position of the center of mass is calculable as: 


r — d - U ♦ Teen 


Where R is the rotation matrix: 


cos(0) — sin(0) 
sin(0) cos(0) 


Lost point procedure In a dynamically changing system, object points are often ob- 
scured. Once a body’s points have been identified, it is possible to calculate it’s position and 
orientation as long as any two points are visible. The body manager’s lost point algorithm 
makes this possible. The technique is quite simple: at initialization time, the a, vectors 
and Wi weights are calculated not only for the “all targets visible” case, but also for every 
possible configuration of two or more visible targets. This calculation is rather complex, 
but it need only be done once, during initialization. The position and orientation algorithm 
described above is then provided with a data structure which describes the currently visible 
target configuration. This structure is updated as the body’s points are lost and re- found. 
The entire set of points need only be visible long enough for the body to be identified. The 
only effect of occluded points is a slight degradation in the accuracy of the position and 
orientation estimates. 

If fewer than two points are visible, then the body is truly lost. When this happens, 
the lost point algorithm notifies the system that the body is lost, and installs a ROI at the 
body’s last known position. The position of the ROI is updated for a short time, based on 
the body’s last known velocity. In most cases, this causes the body to be re-found as soon 
as it becomes visible again. 
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Reporting positions to the user interface The final task of the body manager is 
to provide position updates to the user interface. The names and positions of all tracked 
objects are reported to the user interface whenever requested. This activity is executed as 
a background task, as it is of low priority. 

2.8.9 The Observer Service 

The object managers convert the raw point positions to the more useful object positions. 
However, many functions of the system require estimates of object velocities as well. The 
observer service allows each body manager the option of installing a simple linear state 
estimator [11] to provide object position and velocity estimates. By utilizing a model of the 
plant dynamics, the estimator also provides a one-step prediction of the object position, 
thus compensating for the inherent delay in the CCD camera sensing system. 

The observer system implements the equations: 

x fc = X fc + L(y k - ff5t k ) (2.6) 

X fc+1 = + Tu fc (2.7) 

Equation 2.6 is referred to as the measurement update, and equation 2.7 is the time update 
equation. This is a predictive estimator; x provides an estimate of the state one sample 
period after the last measurement. One independent set of these equations is executed for 
each degree-of-freedom for every moving object in the system. The plant dynamic model 
for our frictionless rigid-body system is quite simple; each degree-of-freedom is a textbook 
double-integrator plant. The observer gains L were chosen to provide a time constant of 
approximately 0.5 second 18 . This value was chosen empirically; it provides fairly quick 
recovery from unmodeled plant disturbances, yet provides low-noise estimated position and 
velocity. 

2.8.10 Experimental Results 

Point tracking performance The simulations reported in the Fifth Semi-Annual Re- 
port predicted point tracking resolution on the order of l/20 t/l of a pixel (average case). 
These predictions are well supported by the experimental data. A typical time history data 
sample of a single stationary point location is presented in Figure 2.35. The ordinate axis 
units are pixels in the vertical direction. The point’s vertical position is indeed stable to 
about l/20 t/l of a pixel. 

Our simulation also predicted that resolution was a rather strong function of target size, 
with the maximum obtained when the target radius was about 3 pixels. This prediction was 
also confirmed. Since the camera’s resolution, 440 x 240 pixels, is greater in the horizontal 
direction, the circular targets cannot have the same pixel radius in both directions. While 
the targets radius is about 2.8 pixels in the vertical direction, it is nearly 4.5 pixels in the 

18 The observer poles are Butterworth at 0.5 Hz 
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Figure 2.35: Typical Point Position Calculation Noise 


horizontal direction. As a result, the horizontal resolution is slightly worse, about l/10 t/l 
pixel 19 . 

Since a vertical pixel corresponds to about 5 mm, and a horizontal pixel to about 3 
mm, the actual resolution in both dimensions is about 0.3 mm. The field of view is about 
1 square meter. Thus, this represents 3000 : 1 resolution — quite acceptable for our task. 

The system’s global accuracy was not carefully measured, but is clearly limited by the 
optical distortion in our rather wide-angle lens. The lens is advertised to suffer distortion 
of about 1% over its field of view; this corresponds roughly to the distortion experienced. 
Resolution is far more important to our tasks than global accuracy; precise manipulation 
of adjacent objects requires only accurate difference measurements. 


Observer performance The observer performance is depicted in Figure 2.36. These 
data were produced by bouncing the object off a piece of rather stiff foam. This causes a 
“step” change in the object velocity. Note that the recovery to the correct state is rather 
quick, and exhibits the expected Butterworth overshoot characteristic response. 

Changes in the estimated state values each sample are readily apparent in Figure 2.36. 
The noise in the observed measurements is actually extremely small; velocity measurements 
for the approach in Figure 2.36 vary by only 1.5 mm/second. It is this high accuracy that 
permits the “catch” algorithm, described below, to predict accurately where the object will 
be in the future. The trajectory still requires periodic updates; even the 1.5 mm/second 
error, when projected 5 seconds forward, produces a 7.5 mm error in predicted position — 
enough to miss the gripper port. 

19 There may be other effects at play here as well, such as horizontal sampling jitter. They were not 
investigated. 





60 


CHAPTER 2. FIXED-BASE COOPERATIVE MANIPULATION EXPERIMENT 




Figure 2.36: Observer Performance 

An example: the catch The most demanding task required of the vision system is 
the interception and capture of a moving object. A “strobe” sequence picture of a typical 
catch is presented as Figure 2.37. The vision system must provide high-speed data for 
three subsystems to perform this task: the two arms, and the body manager. Each arm 
is under vision-guided endpoint control. The desired trajectory for each arm takes it from 
its initial “home” position, to an intercept state that matches the object’s gripper port in 
both position and velocity. 

To perform a successful capture, the arm endpoint must then be held over the gripper 
port for the duration of the time required for the gripper mechanism to engage. The 
positioning must be fairly accurate. A schematic side view of the gripper mechanism is 
shown in Figure 2.38. To engage the port, the gripper is driven downward by a pneumatic 
cylinder. Since the bevel in the port is only 4 mm, the position of the arm must track the 
port accurately for the 0.5 seconds to 0.9 seconds required for the downward motion of the 
gripper. 

Figure 2.39 shows the vertical positions and velocities of the right arm and right gripper 
port during the above catch. The object is being accelerated toward the arm system for 
the first second. During the next second, the arm tip accelerates to match the port position 
and velocity at about the 3.2 second mark. The gripper’s downward motion occupies the 
next second, after which the object is brought to a halt. After the grippers have engaged 
(at about 4.5 second mark) the observer has an incorrect plant model; this accounts for the 
apparent difference in position and velocity after the capture. 

This figure also underscores the importance of the predictive estimates; a l/60 </l second 
delay in the position estimate at the 0.2 meters/second intercept velocity is 3.3 mm. This 
is nearly enough to cause the arm to miss its mark. Any additional accumulated errors 
would ensure a missed attempt. 
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Catch 



Figure 2.37: A Two-handed Catch 



Figure 2.38: Gripper Schematic 
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Figure 2.39: Catch Trajectory Matching 

The task is complicated by the need for both arms to arrive at their intercept locations 
simultaneously. The arm trajectories are also updated as the object position and velocity 
estimates improve. The strategic control section below provides the details of both these 


issues. 
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2.9 Future Work 

The fixed-base cooperative manipulation experiment is now essentially complete. During 
the next report period, the technology developed on this facility will be transferred to the 
floating base experimental apparatus. 

We are also beginning the technology documentation process in earnest. During the 
next period, we expect to publish several technical papers — as well as a Ph.D. thesis — on 
the results of this effort. 


Chapter 3 


Multiple Arm Cooperation on a 
Free-Flying Robot 

Ross Koningstein 

3.1 Introduction 

This chapter summarizes the work performed on multiple arm cooperation on a free-flying 
robot under NASA grant NCC-2-333 for the period February 1988 to August 1988. Mul- 
tiple arm cooperation from a free-flying robot is one of the basic technologies required for 
space based manipulation. Ongoing work at the Stanford Aerospace Robotics Laboratory 
is yielding insights into the fundamental nature of free-flying manipulator robots, both in 
the understanding of the dynamics, and the understanding of the control problem. Ex- 
tensions have been made to previous work on the dynamic modelling of a typical space 
robot configuration, the kinematic chain, to demonstrate the simple formulation of desired 
endpoint accelerations for control purposes. Also, given the joint accelerations, the compu- 
tation of the control torques can be computed via an efficient 0(n ) algorithm. The control 
systems being considered for implementation are based on the computed torque formulation 
introduced by Craig[9]. 

3.2 Motivation 

Computed torque formulations of the past have found the redundant degrees of freedom 
possessed by floating base robots awkward to deal with. Standard computed torque schemes 
rely on the inverse and derivative of the system’s Jacobian, J as expressed by 

y endpoints _ 

where v is a vector of the speeds of the manipulator endpoints, measured in some coor- 
dinate system and q are the derivatives of the system generalized coordinates. When this 
conventional approach is taken to the formulation of the Jacobian of a free-flying robot, 
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it is non-square[2] and the standard techniques to solve for the desired joint accelerations 
cannot be used. We will demonstrate that a square Jacobian for a floating based robot can 
be formulated, and that the joint accelerations can be solved for in a manner similar to the 
standard technique. 

Continuing work in the analysis of robot dynamics by Rosenthal[31], Rodriguez[30] and 
others have shown that robot dynamics can be solved in 0(n ) computations. The ability to 
solve the inverse dynamics equations for control torques in 0(n) computations would give 
the controls engineer the same benefits available to the simulation community: tractability 
of large problems. Our work has led us to develop an O(n) algorithm for control torque 
calculation suitable for kinematic chain robots. 


3.3 Free Flying Robot Jacobian 

The space robot being considered falls into the class of objects called kinematic chains. 
The mathematical model for kinematic chains has a special structure allowing an algorithm 
to easily formulate endpoint acceleration equations. The work on the formulation of the 
Jacobian for a free-flying robot draws on previous work[7], which formulated equations of 
motion, endpoint accelerations and closed chain constraint equations. Notation used is that 
introduced by Kane[18]. 

This analysis is performed for a free-flying robot torso with one or more kinematic chain 
manipulator arms. The derivations are for 3D systems, and have been specialized to 2D for 
our experimental work. A generic free-flying robot has n degrees of freedom, which account 
for the 3 degrees of freedom of each arm, and 6 degrees of freedom of the robot’s torso. 
The robot’s torso’s degrees of freedom are not directly controllable by the arm torquers if 
we wish to achieve arm endpoint control. 

3.3.1 Concepts used in Analysis 

This theory for serial chain manipulators is derived using Kane’s dynamical analysis tech- 
niques. The analysis that follows assumes that the velocities v of points and angular 
velocities u> of bodies in the system under consideration can be expressed in a Newtonian 
reference frame as follows: 


V' = X>>, 

3 =\ 

V 

u>‘ = u, 

4=1 

where the generalized speeds u\„ n are linear combinations of the derivatives of the gener- 
alized coordinates q\.. n . This will be true if no part of the system is undergoing prescribed 
motions. The partial angular velocities of bodies, and partial velocities of points, as defined 
by Kane[18], can be shown to be: 
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d 



3.3.2 Jacobian Element Equations 

In this endpoint acceleration control specifications will be expressed in terms of joint speeds 
and accelerations. The standard computed torque method, which relies heavily on the 
Jacobian, will be briefly overviewed. We wish to be able to determine the Jacobian scalars 
j ra which form a Jacobian matrix as follows: 

v endpoint = 

The endpoint acceleration can then be expressed as: 

ae ndpoint = J . in+ j UiB 

and the joint accelerations are typically solved for by rearranging these equations: 

«l..n = J-V ndP ° int - J «!..») 

This matrix equation can be broken down into components which are dependent upon 
the partial velocities and partial angular velocities of the endpoint of the each kinematic 
chain in the system. Each endpoint velocity can be expressed in terms of its partials as: 

n 

y endpoint __ ^ endpoint^ 

r=l 

and therefore endpoint velocity can be expressed in terms of speeds along some established 
inertial x,y and z directions, for example, along unit vectors which we define as x, y and z: 


v endpoint 


£ v r endpoint • X u T 

r= 1 
n 

v endpoint _ 

= 

J2 v“ dpoint • y U T 

r— 1 
n 

v endpoint z 

— 

V* ndpoint • Z u r 

r=l 

the elements of the Jacobian due to 

the 

endpoint velocity values are therefore: 

hr 

— 

v endpoint x 

hr 

= 

endpoint # v 
v r * J 

hr 

= 

v endpoint # z 
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These partial velocities axe the same as the ones used in the dynamical derivations, 
which were discussed in previous work[7]. 

Endpoint acceleration control specification can be expressed in terms of the Jacobian, 
its derivative, and the generalized speeds and their derivatives: 

a endpoint _ n + j ttl n 

The derivatives of the elements of the Jacobian can also be determined from quantities 
used in the dynamical equation formulation: 


jlr 

-endpoint 

v r 

hr 

-endpoint 

~ v r 

hr 

-endpoint 

““ v r 


If we assume a system, S, consists of a free-floating kinematic chain, with robot arms 
which possess 3 independent links with controls, then each endpoint will possess 3 trans- 
lational degrees of freedom and the torso will possess 6 degrees of freedom, since it is free 
to translate and rotate in space. We are able to specify the desired endpoint acceleration, 
but not the torso motion. The joint accelerations cannot be solved for purely by knowing 
the endpoint motion, as is possible on a fixed base robot. When controlling a free-flying 
robot, the motions of the torso must be accounted for. These motions, expressed by the 
generalized speeds and coordinates of the torso, need to be solved simultaneously with the 
motions of the arms. This can be done by including additional equations which ensure 
system consistency given its dynamic properties. 

3.3.3 Jacobian Augmentation Equations 

The Jacobian needs to be augmented with several equations to reflect the relations between 
joint speeds in a free-floating kinematic chain. Constraint relations can be used in this 
manner to solve joint accelerations in constrained systems (ie. closed chain), as reported 
earlier[7], or they can be used to solve underdetermined systems, such as free-floating robots 
with redundant degrees of freedom. In this section, we will add equations to the Jacobian in 
order to be able to solve the case of the underdetermined system. Augmentation equations 
equations could be taken directly from the system dynamics, however, they contain motor 
torques which axe not known a priori. Instead, we will investigate the linear and angular 
momenta conservation equations. The linear and angular momenta in free flying robots axe 
either conserved quantities, or vary according to the settings of system thrusters. We will 
assume that these thruster settings are known a priori. 

First, the linear momentum, then the angular momentum of the system will be exam- 
ined. The linear momentum of body i in the system is 
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where the partial linear momentum of body i is defined by 


L t « 

a m v « 


The linear momentum of system of a system of v bodies is the sum of the linear momenta 
of each body i in the system: 


L k ±V 
1=1 

= 

1=1 
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1=1 5=1 
v n 
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1 = 1 5=1 
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where the partial linear momentum of the system is defined by 

i.^E^vr 

i=i 

The partial linear momenta of the system can be formulated using the system masses 
and center of mass partial velocities. The integration of these vector quantities into the 
Jacobian is similar to the process used for the partial velocities discussed in the previous 
section, and will be discussed further at the end of this section, after the angular momentum 
terms are examined. 

The angular momentum of each body i, about its center of mass is: 


70 


CHAPTER 3. MULTIPLE ARM COOPERATION 


3=1 
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where the partial angular momentum of each body is 


H* £ r'/*x 


The angular momentum of the system, consisting of v bodies, about the system’s center 
of mass point, is: 


V V 

H = J^H* + £(r»* - O X mV* 

t=l 1 = 1 

V 

= V + (r‘* - r cm ) X m’v**) 

t = l 
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t=l 3=1 3=1 
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where the partial angular momentum of the system is then 


H. = £(Hj + (i* - r~) x l;) 

1=1 

The equations which describe the partial linear and angular momenta can now be used 
to augment the system Jacobian. This will result in a full rank Jacobian, allowing the joint 
accelerations to be solved for. The resultant Jacobian when the system’s linear and angular 
momenta are taken into consideration is: 
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The partial linear momenta can be normalized by the sum of the masses of the bodies 
in the system. These normalized quantities have the same dimensions as partial velocities, 
and can be treated just like the partial velocities already forming part of the Jacobian. The 
normalized partial linear momenta are defined as: 


/ 


3 


A 



The convenient feature of this normalization is that these normalized partial linear 
momenta of the system for the indices which describe the translation of the torso, s = 
v, v + l,i/ + 2, are unit vectors equal to those appearing in every endpoint partial velocity. 
By performing simply subtracting the normalized partial linear momenta rows from the 
endpoint partial velocity rows, the size of the Jacobian can be reduced by three rows, 
creating a reduced set of gi.. 3„_3 to solve for. 


3.4 Order n Inverse Dynamics 

In this section we will demonstrate a simple and straightforward algorithm to solve the 
inverse dynamics equation for the control torques along a serial chain. Traditional computed 
torque control schemes have used the following equation to compute the joint torques: 

Mu = Nu + PT 

This method requires 0(n 2 ) computations, and requires that the mass matrix and non- 
linear terms of the system S be computed, then desired joint accelerations and known joint 
rates be used to generate a vector from which the control torques are easily derived. We will 
present an alternate method of computing these joint torques in o(n) computations. This 
method is based on the Newton-Euler method of formulating robot equations of motion, 
but instead of generating equations symbolically, we will generate numerical values for 
accelerations, joint forces and torques, and actuator torques as we traverse the robot’s 
chain manipulator. 
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The algorithm consists of two phases: 

First, the joint accelerations are used to determine the accelerations of all the joints 
and each of the center of mass points of the v bodies in the system. We can use the link 
recursion relation that the acceleration at the end of a link is related to the acceleration at 
the start of a link as follows: 

^end _ ^start _j_ ^iink ^ ^start to end ^link ^ ^link ^ ^start to end 

where the following components are derived as follows: 

a linlc i = Q Unk i-X + £ . A i 

The axis direction A* is a positive rotation, in a right handed sense, along <7;. The torso 
center of mass acceleration can be constructed using u„,„+i,i,+2» while the torso angular 
acceleration can be constructed using u l/+ 3 iJ/+ 4 iV+ 5. All other accelerations in the system 
can be determined using the torso accelerations and the link recursion relations. 

Second, the forces and moments are propagated back from the end of each chain. We 
assume the force and moment at the end of the chain is a known value (typically zero 
at the end of the arm). We take moments about the joint at the start of the link, and 
consider only the components along the joint’s axis A*. The moments due to the center 
of mass acceleration and the link’s angular acceleration are easily evaluated given its mass 
properties. The joint motor torque will be the only unknown in the equation 

T* • A* = en< * + r start *° en< * x F cnd — r start to • x m*a**) • A* 

Now take moments about the link start point, which are the moments applied to the 
end of the next link in. Likewise, the sum of the forces will yield the forces applied by 
this link to the end of the next link in. The focus of reference can now be shifted to the 
next link in, where this process can be repeated until all of the control torques have been 
determined. If linear actuators are being used, then the actuator force solution is done 
using the sum of forces along the actuator axis. 

If the cha'" is closed, then a ’squeeze’ force can be assumed as a starting internal force 
at the link end by conceptually cutting the closed chain, and the same procedure can be 
followed but with the two chains generated by the cut. 

The process of solving for the joint control torques or forces is fairly straightforward, 
and if the robot has two or more arms, the solution for the control values for the various 
arms can be done in parallel. 

3.5 Status 

The Jacobian formulation method introduced here has been used to generate the joint ac- 
celeration specification matrix equation necessary in order to solve the computed torque 
control problem for the general 3D case of a free-flying robot with kinematic chain manip- 
ulators. The 0(n ) inverse dynamics solution has also been derived for this general 3D case. 
A specialized and partially optimized derivation for 2D has been done to allow testing on 
our experimental free-flying robot model. 
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3.6 Further Research 

The underlying theory will allow position control of the endpoints of robot arms on a free- 
flying robot platform, for the purposes of positioning and reaching out to grasp and hold 
an object. The methods presented here along with work previously presented[7] will yield a 
controller capable of position and force control for the closed loop ’capture’ configuration. 
The algorithms presented generally require O(n) computations, except for the solution for 
the values of the joint accelerations required for the desired control, which require 0(n 2 ) 
computations. This last computational bottleneck will be examined over the course of 
our further research. Our physical simulation, a two dimensional satellite robot simulator 
which floats on an air bearing, is very near completion, with initial experimental results 
being presented January 1989. 


Chapter 4 


Navigation and Control of 
Free-Flying Space Robots 

Marc Ullman 

4.1 Introduction 

This chapter summarizes the progress to date in our research on global navigation and 
control of free-flying space robots. This work represents one of the key aspects of our 
comprehensive approach to developing new technology for space automation. Ultimately, 
we envision groups of fully-self contained mobile robots making up the core work force in 
space. 


4.1.1 Motivation 

Although space presents us with an exciting new frontier for science and manufacturing, 
it has proven to be a costly and dangerous place for people. Space is therefore an ideal 
environment for sophisticated robots capable of performing tasks that currently require the 
active participation of astronauts. 

While earth based robots have not always proved to be cost effective solutions to man- 
ufacturing inefficiencies (due to the abundance of cheap labor), the tremendous cost asso- 
ciated with putting men in space, especially when EVA is required, makes the economics 
of robots in space particularly attractive. 

4.1.2 Research Goals 

The immediate goals of this project are to: 

• demonstrate the ability to simultaneously control robot base position and arm orien- 
tation so that a free-flying robot can navigate to a specified location in space while 
manipulating its arm(s). 

PRECEDING PAGE BLANK NOT FILMED 


iAfii I WIENTIONALU SUNK 



76 


CHAPTER 4. NAVIGATION AND CONTROL 


• demonstrate the ability to capture a (possibly moving) free-floating target “on-the- 
fly” using the manipulator arm while the base is in transit. 

• provide a suitable platform for the eventual addition of A.I. based path planning and 
obstacle avoidance algorithms which will enhance the robustness of task execution. 

4.1.3 Background 

This work emphasizes the modeling of robot dynamics and the development of new control 
strategies for dealing with problems of: 

• a non-inertially fixed base (i.e. free-floating base) 

• redundancy with dissimilar actuators 

• combined linear and non-lineax actuators 

• highly non-linear dynamics 

• unstructured environments 

Our laboratory work involves the use of a model satellite robot which operates in two- 
dimensions using air-cushion technology. We have developed a series of satellite robots 
which, in two dimensions, experience the drag-free and zero-g characteristics of space. These 
robots are fully self-contained vehicles with onboard gas supplies, propulsion, electrical 
power, computers, and vision systems. The latest generation of robots is also equipped 
with a pair of two-link arms for acquiring and manipulating target objects. 


4.2 Summary of Progress 

The following advances have been achieved during the past report period: 

• The low pressure gas subsystem has been revised to incorporate two-stage regulation 
in order to provide low pressure air for pneumatically actuated end effectors. 

• A revised version of the Power Control Unit (PCU) that corrects the problems asso- 
ciated with the original implementation has been designed and built. 

• The Safety Cut- Out /Solenoid Drivers Board has been designed and manufactured. 

• The board level components for the on-board computer system have been obtained 
along with the VxWorks real-time operating system. Successful operation of the 
computer system has been demonstrated. 

• Initial device drivers have been written and tested to verify the functionality of the 
I/O system. 

• A VME-bus based version of the “point-grabber” vision system used in our earlier 
work has been designed and is currently being fabricated. 
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• An enhanced network architecture incorporating gateways and subnets that facilitates 
off-board processing using multiple parallel processors has been proposed and tested. 

• Our plan to migrate all of our design, analysis, and software development activities 
to a network of Sun Workstations has been proceeding smoothly. 

4.3 Experimental Hardware 

4.3.1 Gas Subsystem 

We have revised the on-board gas subsystem to incorporate several new features including 
a miniature low pressure regulator, a corresponding pressure gauge, and a pair of two- 
way solenoid valves. (See Figure 4.1). The solenoid valves are used for controlling the air 
supply to a set of pneumatically actuated end effectors. These end effectors or grippers 
are of essentially the same design as that used in the fixed base cooperation experiments 
described in Chapter 1. They incorporate a simple z-axis plunger which is driven by a 
double-acting piston. The new regulator provides the source of low pressure air (10-60 
psi) for operating these pistons while also providing much better regulation of the flotation 
air supply during thruster operation. This later benefit is derived by now using two-stage 
regulation ahead of the flotation control flow meter whereby the second stage of regulation 
isolates the flow meter from the step changes in supply pressure which occur as a result of 
the bang-bang thruster operation. 

4.3.2 Power Distribution Subsystem 

Two new custom printed circuit boards were developed to facilitate operation of the robot. 
The first was a new version of the Power Control Unit (PCU) which supports the following 
improvements: 

• Simplified control circuitry implemented with discrete device logic for much improved 
noise immunity from power-on transients. 

• More convenient operation including the addition of a master power-on switch. 

• Support for independent operation of the battery charging circuits even when the 
remainder of the power distribution system is off. 

The second was a new board that performs the combined functions of providing a 
safety cut-out circuit and solenoid driver logic and amplifiers. The safety circuit operates 
in several different switch selectable modes. In its nominal mode of operation, the absence 
of a computer generated “heartbeat” (presumable signaling that the computer has crashed) 
disables power to the manipulator motors and the thruster solenoids in order to prevent 
damage to the robot or its surroundings. Manual overrides are possible to either enable or 
disable the output circuitry. A capability for remote manual disable, i.e. a “kill switch,” is 
also supported. 
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Figure 4.1: Schematic diagram of satellite robot onboard gas subsystem. 

Printed circuit board implementations of these boards have been manufactured and 
tested and are now operating properly on board the robot. 

4.3.3 Real-Time Computer 

The onboard computer subsystem is now essentially complete and operational. We have 
connected a pair of horizontally mounted five-slot VME card cages via a 96 conductor 
ribbon cable. The “I/O” card cage contains a 16 channel 12 bit A/D board, a 4 channel 
12 bit D/A board, and a 32 bit digital I/O board while the “computer” card cage houses 
a Motorola MVME 147 single board computer. This unit features a 68030 microprocessor 
running at 20 MHz along with a 68882 Floating Point Coprocessor. It also contains 4 MB 
of dynamic RAM, an Ethernet controller, a SCSI bus interface, four serial communications 
ports, a Centronics parallel interface, and a complete VME bus controller. 

We are using Wind River Systems’ VxWorks 1 as our realtime operating system and 
now have a device driver customized for the MVME 147. The computer uses an EPROM 
based boot monitor incorporating this device driver and configuration settings stored in 

1 VxWorks is described in depth in Chapter 3 of our Sixth Semi-Annual Report 
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EEPROM to boot over the Ethernet network connected to our Sun 3/160 file server. 

4.3.4 I/O Interface Modules 

Device drivers incorporating basic functionality have been written and tested to demon- 
strate the proper operation of the I/O system. These drivers are non-interrupt driven 
C-callable routines. Each device driver typically has three callable functions: An initializa- 
tion call, a read procedure, and a write procedure. The drivers for output devices (DO and 
D/A) save the current state of the outputs (when the hardware does not) thus enabling the 
read calls to return the current output states. 

4.3.5 Point Grabber Vision System 

A new version of the “point-grabber” vision system used in our earlier experiments has 
been designed which incorporates the following new features: 

• A VME bus compatible interface. 

• The ability to handle up to four cameras simultaneously. 

• Jumper selectable generation of sync signals for cameras which allow for external 
syncing. 

• Programmable adjustment of the threshold setting 

A wire-wrap prototype has been built and debugged to the point of demonstrating basic 
functionality; however, it has yet to been used with higher level code to preform actual 
vision services. 

A revised version is being designed using a schematic capture and PCB layout system. 
As a PCB it will be feasible to produce boards for each robot as well as for possible off-board 
vision. 


4.3.6 Multilayer Network Architecture 

In light of the fact that VxWorks supports a complete implementation of the TCP/IP 
networking protocol including gateway and subnet capabilities, we have devised a multi- 
layer network architecture that allows us to do parallel processing using both on-board and 
off-board computers. 

Figure 4.2 illustrates the topology of our three levels of subnets. Our main laboratory 
network (ARL-Net) connects our diskless Sun Workstations and our VxWorks gateway 
machine(s) to our central file server. 2 Our VxWorks gateway machine consists of an open 
frame 20 slot VME card cage and a pair of MVME 147 single board computers 3 each with 
an onboard Ethernet controller. A virtual network implemented in software using shared 
memory transparently connects the two MVME 147s together (ACV-BP-Net). The second 


2 This fileserver also serves as a gateway to the campus wide network. 

3 These are the same computers as described above in the section entitled Real Time Computer. 
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MVME 147 serves as gateway to a third network (ACV-Net) 4 connecting to the on-board 
computer. 
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Figure 4.2: Distributed Realtime Computer System Network Topology. 

This topology offers a number of benefits: 

• Since these devices support TCP/IP as well as NFS, all communications between 
processors axe transparent at the socket, RPC, or NFS level. This means that software 
can be constructed (except for timing considerations) without regard to whether two 
processors axe in the same card cage or even on the same network. Once the system 
is configured, all routing though the gateway machines is completely transparent to 
the user. 


• By isolating the communications between the on-board and off-board processors with 
a private subnet, the likelihood of collisions is greatly reduced due to the limited 


4 This network is to be implemented using & fiber optic Ethernet to minimize the effects of the cable on 
the robot dynamics. Ideally the link would be made using RF transmitters and receivers; however, we have 
been unable to find a low cost system capable of handling the 10 Mbps bandwidth with an acceptable level 
of reliability 
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access hence improving the real-time performance of the network. 5 

• As we extend our work from cooperating manipulators to cooperating robots it will be- 
come necessary to provide a communications channel between the cooperating agents. 
The proposed network topology supports this requirement in a very natural manner 
via the simple addition of new nodes (one for each new robot) to the third level 
(ACV-Net) network. The robots will then be able to communicate with each other 
as well as with the off-board computers that may be providing global coordination 
and planning. 

• A desire to do sophisticated planning and obstacle avoidance will naturally lead to a 
requirement for greater off-board processing power. This need is also met in straight 
forward manner through the addition of more processor boards to the second level 
(ACV-BP-Net) or backplane network 

The topology described above has been set up and configured and is now operating suc- 
cessfully. As the first gateway processor boots, it is configured as a gateway machine. This 
in turn allows the successive subnet computers to boot transparently through the higher 
level gateway machines. 


4.4 Real-Time Development System 

Our entire laboratory has been in process of migrating all of our software development, 
testing, downloading, and debugging activities to a network of Sun Microsystems diskless 
workstations. The network environment, based on an Ethernet LAN, couples our com- 
puters together so that all software and data can be shared transparently. This facilitates 
technology transfer and enhances information sharing among the many different projects 
in our laboratory. 

The selection of the VxWorks realtime operating system software package is one of the 
important corner pins making this consolidation possible. To date, the transition has been 
proceeding smoothly. We have sold our pair of VaxStation II’s and upgraded three Fujitsu 
Eagle Disk Drives to a pair of new 2382 drives, thereby nearly doubling our on-line storage 
capacity while improving performance and reducing maintenance costs. We hope to be 
able to upgrade our server and continue to add new workstations to the network as our 
computing requirements increase. 


5 Because Ethernet is a CSMA/CD based network with uses a combined exponential and random backoff 
algorithm to avoid collisions it offers no guaranteed upper limit on packet delivery time; however, as network 
traffic is decreased, the statistical likelihood of an unresolvable collision is greatly decreased. 

It should also be noted that it is our intent to use off board computers in a client/server paradigm whereby 
a delayed response would not catastrophic. That is to say, asynchronous processes would be running on the 
off board computers that would not be tied “lock-step” to the real-time control loops running on-board. 
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4.5 Modeling and Simulation 

The complete dynamical equations of motion have been derived and verified for a single- 
armed version of the robot. These equations have been coded up and simulated for both 
free and forced motion. 


4.5.1 Analytical Model 

The robot has initially been modeled with only one arm, since the global control and target 
capturing problems can be addressed with this somewhat simpler configuration. (See the 
section on Multi- Arm Cooperation for a derivation of the equations of motion for the two- 
armed version.) The model consists of three planar rigid bodies connected by two torque 
motors. (See Figure 4.3). The base body is capable of translation and rotation in the plane 
via eight on-off-on thrusters mounted as 90° opposed pairs on each of four corners. 



Figure 4.3: Free body diagram of space robot indicating nomenclature used for dynamic 
modelling. 
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4.5.2 Equations of Motion 

The equations of motion for this five-degree-of-freedom system were derived using Kane’s 
method[19] and for verification purposes were also derived using the symbolic equation 
generation program SDEXACT[32]. The joint space equations of motion can be expressed 
in terms of a vector of generalized coordinates q (corresponding to the joint positions and 
angles) and a vector of generalized speeds u. They are of the form: 


F r + F* = 0 


or 


F - Af(q)u - V"(q, u)u = 0 


where 


F* = — (ilf(q)u + V(q, u)u) 

M( q) is the configuration dependent mass matrix, F(q, u) is the configuration and velocity 
dependent matrix of non-linear terms, and F is the vector of generalized active forces. The 
u’s or generalized speeds are defined in terms of the state derivatives, q, by the relation 


u = Yq 


where 


Y = 


1 0 0 0 0 
0 10 0 0 
0 0 10 0 
0 0 110 
0 0 111 


In order to implement a simulation, we must solve the previous set of equations to 
obtain 


q = F -1 u 

u = M(q)- 1 (F-F(q,u)u) 

4.5.3 Operational Space Computed Torque Controller 

As discussed in our fifth Semi-Annual Report, a computed torque[9] controller was imple- 
mented as a first cut at closed-loop control. This controller has since been extended to 
operate in “operational space” [21] so that trajectories axe now specified in terms of the 
manipulator tip positions rather than in terms of joint angles. This later approach is much 
preferred when one is generating trajectories with a path planning algorithm. 

The extension of the computed torque controller into operational (or cartesian) space is 
fairly straight forward. We begin by defining a state vector of coordinates which describe 
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the robot configuration in terms of variables we are directly interested in controlling. In 
our case, we have selected 

x =[ xb c VB C Ob xti P yu P ] T 

where (xb c ,Vb c ) is the position of the center of the base in inertial space, 9b is the orien- 
tation of the base in inertial space, and (x tip, y tip) is the position of the end point of the 
manipulator in inertial space. A set of basic kinematic equations relates this state vector 
to our original set of generalized coordinates q so we can write: 

x = KIN(q) 

Typically one defines the relation between the time derivatives of these two state vectors 
in inertial space as the Jacobian yielding: 


X= Jq 

However, since our equations have been cast in terms of generalized speeds, u , we find 
it more convenient to make the following definition: 

J = JY~ X 


so that 

x = JY~ x u = Ju 

Differentiating this relationship leads to 

x = ju + J u 


from which we can solve for u 


u = J X {-j u + x) 


We can replace x with a desired acceleration &d e3 composed of both feed forward and 
feedback terms resulting from our commanded trajectory and our feedback control law re- 
spectively. With a simple proportional-derivative (PD) control law our desired acceleration 
vector consists of the following terms 

a des = *cmd + - x) + Kjx^d - x) 

where K p and K v are diagonal matrices containing the proportional and derivative 
feedback gains respectively. 

Substituting the resulting expression for u back into our original equations of motion 
yields a set of generalized forces which represent our “inverse dynamics” control vector of 
forces and torques. 
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F = M(q){ J 7~ 1 (-ju + a (iea )} + F(q,u)u 

= M(q)J~ l &de3 + [-M(q) J~ l j + V(q, u)]u 

The resulting generalized forces can then be mapped onto the available actuators (thrusters 
and arm torque motors) using the pseudo-inverse and thresholding techniques described in 
our previous report[6j. 

4.5.4 Trajectory Generation 

Currently we are using commanded trajectories in the form of time and amplitude scaled 
unit step fifth order polynomials. Now that we have implemented an operational space 
controller, these trajectories are described in our cartesian work space rather than in joint 
space as was required with our joint space controller. Fifth order polynomials were selected 
so that we can match a desired position, velocity, and acceleration at both ends of the 
trajectory. The trajectory specification is given as a matrix T apec of the form: 

T Jpec = [ Xo Xo tf Xo t) X/ Xftf Xjt) 

The actual trajectories are then computed using the equations 

Xcmd = ar s + 6r 4 + cr 3 + dr 2 + er + f 
Xcmd = (5ar 4 + 46r 3 + 3cr 2 -f 2dr + e)/tj 
Xcmd = (20ar 3 + 126r 2 + 6cr + 2)/t 2 

where r = t/tj is normalized time and [a b c d e / ]isa vector of coefficients 
given by 

| a b c d e f ] = T spec C~ l 

where C is the matrix of constant coefficients that results from evaluating a generic fifth 
order polynomial and its derivatives at 0 and 1. 

4.5.5 Simulation Results 

In order to verify the effectiveness of the control strategy outlined above, several test cases 
were run in simulation. The algorithm was coded up as a series of “M-files” using the 
matrix manipulation program Matlab. Figure 4.4 shows a simulation run in which the 
robot base was commanded to move from the origin to a location 1 meter to the right and 
1/2 meter up while undergoing a 90 degree counter-clockwise rotation over a 10 second 
time interval. At the same time the arm tip was commanded to move from (-0.25, -0.5) 6 
to (1.5, 0.5) and to arrive with a velocity of lOcm/s in the positive y direction. Figure 4.5 

6 A11 coordinate pairs refer to dimensions in meters relative to the origin. 
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shows the time history of the actuator forces and torques used for this trajectory. It shows 
the resulting pulse width modulation (PWM) of the eight thrusters along with the zero 
order hold (ZOH) torque outputs for the manipulator motors. Thus these results show 
that given a “reasonable” trajectory, we can control both the base and the manipulator 
using the control formulation presented above. 



Figure 4.4: Time-lapse plot from simulation of space robot executing combined base and 
manipulator motion under closed loop control. 


4.6 Summary 

Significant progress has been made during the past report period on both the hardware and 
the analysis fronts. We now have an operational real-time computer system that supports 
remote login, remote debugging, multiple processors, distributed processing, and transpar- 
ent networking. We have demonstrated successful operation of the gas subsystem (with 
its improvements), the power distribution system, the I/O subsystem, and the prototype 
vision system. 
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time (sec) 

Figure 4.5: Thruster and torque motor time histories used in executing space robot trajec- 
tory shown above. 

4.7 Future Work 

Our robot is ever closer to becoming operational. All major subsystem components are now 
in place. The principle work remaining entails wiring and interconnecting the vast array of 
components. Specific items yet to be completed include: 

• Completing the I/O Transition Module — an interface/patch board that provides a 
generic connection between the I/O Subsystem and the Analog Subsystem while pro- 
viding custom configuration and signal breakout /monitoring. 

• Wiring the sensors and actuators. 

• Installing the manipulator end effectors (grippers). 

• Mounting the onboard and off-board cameras. 

• Mounting the angular rate sensor and the x-y accelerometers which will serve as the 
core of an onboard INS system for tracking vehicle position and orientation. 
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The modular design philosophy, which has been a guiding principle for this project since 
its inception, will continue to apply as our focus begins to shift away from hardware toward 
software. 


Chapter 5 


Multiple- Vehicle Cooperation 


William C. Dickson 


5.1 Introduction 

This chapter introduces a new line of research being conducted in the area of multiple- 
vehicle cooperation. This work will eventually unite the various lines of research presently 
being conducted in fixed- and floating-base cooperative manipulation, and in global navi- 
gation and control of space robots. Our goal is to demonstrate multiple free-floating robots 
working in teams to carry out tasks too difficult or complex for a single robot to perform. 
Achieving this cooperative ability will involve solving specialized problems in dynamics and 
control, high-level path planning, and communication. 


Progress Summary 

Activities completed from March 1988 to August 1988 were: 

• Far-range research goals were defined 

• Initial model of the physical system was developed 

• Candidate path/motion planning algorithm was developed 

• Closed-loop control for two- robot manipulation was successfully simulated 

5.2 Research Goals 

Some of the goals of this project are: 

• Cooperative manipulation and assembly by multiple robots 

• Fine cooperative manipulation in presence of on-off control 
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• Development of control strategies for path following in presence of obstacles and large 
disturbances 

• Path generation considering dynamic constraints and known geometric boundaries 

• Task planning for complex assemblies 

5.3 Experimental Hardware 

The vehicles to be used in this research are a pair of the two-armed free-floating robots used 
in the LEAP experiment (see Chapter 6). As with the Navigation and Control research, only 
one of the two arms on each of the robots will be used, removing the issue of cooperation 
between arms on the same robot and allowing the research to focus on cooperation between 
independent vehicles. 

The vehicles will be manipulating a free-floating object having either two receptacles 
for vehicle dockings (as with the object used in the fixed-base cooperation research) , or a 
bar to be grasped by grippers similar to those used in the LEAP experiment. The mass 
and inertia of the object will be adjustible so that studies can be made concerning a given 
controller’s performance over a range of these parameters. 

Experiments will be performed on the 9’ x 12’ granite table. This large size is necessary 
if the robots and manipulated object are to perform interesting configuration changes or 
maneuvers ( such as moving around obstacles ). 


5.4 Modelling 

The robot is modelled as a three-link chain consisting of a free-floating base and a single 
two-link arm — resulting in the five-degree-of-freedom system described by $(t=l,. . .,5) as 
shown in Fig. 5.1. The set of actuators consists of eight on-off thrusters (mounted as 90° 
opposed pairs on each of four corners of the base), a momentum wheel on the base, and 

torque motors at the shoulder and elbow of the arm. For modelling and control purposes, 

the thrusters are grouped into two perpendicular, multi-directional, on-off-on sets. With 
this simplification, base control forces and torques are conveniently separated between the 
thrusters and momentum wheel, respectively. Thus, each vehicle has five controls — two 
thruster forces (F\ and Fi) for two-dimensional translation, and three torques (Ti, T2, and 
T3) for orientation. f x and f y are manipulation forces. 

The Equations Of Motion (EOM) for the five-degree-of-freedom system were derived 
using Kane’s method [18] and can be written as: 

Mu = b + Gr + Hf (5.1) 

q = u 

where q, u, t, and f axe defined as: 

q = [ <7i 92 ?4 Qs ] T , 
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Figure 5.1: Modelling of single-arm vehicle 
u = [ U\ «2 U3 U 4 u$ ] T , 

r = ( Fi Fi 7i r, r 3 ] T , 
f = [ 

5.5 Controller 

Computed torque controllers solve systems’ EOM for the forces and torques required to 
produce desired accelerations. With the present system, given by the EOM in ( 5.1), G is 
always invertible so that the r necessary to produce the desired acceleration vector 
can be given as: 

r = G -1 (Mu de4 - b - Hf). 

However, in our case, the first two elements of r, F\ and Fz, are available only in the 
discrete values of 0 and ±F max , thus not all Ud e4 can be produced. More precisely, only 
three linear combinations of the five terms in u^ ea can be arbitrarily specified — the other 
two vary discretly due to the on-off-on values of F\ and F^. This problem of specifying 
desired accelerations was resolved as follows. 

First, we define a new velocity vector v : 

v = [ ui u 2 u 3 v x v y ] T , 

where v x and v y are the manipulator tip speeds in the x and y directions. Defining a as 
the time derivative of v, 

a = v = [ u x u 2 u 3 a x a y ] , 
we find that a can be written as 

a = Ru + s, 
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and since R is invertible, 


u = R x (a — s). 


We can now rewrite ( 5.1) in terms of a: 


MR” 1 (a - s) = b + Gr+Hf. 


Using the substitutions 


N = MR" 1 , 
c = b + Ns, 


we arrive at a new set of EOM expressed in terms of a: 

Na = c + Gr + Hf. (5.2) 

The motivation for writing the EOM as in (5.2) is that the acceleration vector a now 
contains terms that directly describe the motion of the manipulated object — namely a x 
and a y , the manipulator tip accelerations in the x and y directions. Precise object control 
requires that arbitrary values of these accelerations be achievable. As before, only three of 
the five terms in a can be arbitrarily chosen. With two being the important tip accelerations 
a x and a v , the remaining choice is the base angle acceleration U3. 

Thus, a can be partitioned into determined and arbitrary parts: 

a = [ af | & 2 ] T = [ «i «2 | «3 a* a y ] T . 
r, N, and G are similarly partitioned: 

r = [ T f I t 2 ] T = [FiF 2 \T 1 T 2 T 3 ) t , 


N = [N x |N 2 ], 

G = [ Gj | G 2 ]. 

We can recombine the equations in (5.2) according to these partitionings to arrive at: 



ai 


Ti' 

[Ni| - G 2 ] 

T 2 _ 

= c + [ Gi | - N 2 ] 

_ a 2 _ 


Defining W and P as: 


+ Hf. 


W = [Ni| - G 2 ], 
P = [Gi| - N 2 ], 


we arrive at the EOM expressed in a convenient form for control purposes: 
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Figure 5.2: Schmitt trigger used to determine thruster forces 

Given the discrete thruster forces T\ and the desired accelerations a2, we can determine 
the resulting base accelerations aj and the required control torque vector r 2: 


’*i" 

= W -1 (c + P 

Ti' 

. T 2. 


_ a 2 _ 


+ Hf). 


(5.4) 


The control problem is thus divided between the separate tasks of first determining ri 
( Fi and F 2 ), then using these values with the desired accelerations a2 (£3 ,a x , and a y ) to 
calculate the motor torques r 2 and the resulting base accelerations ai. 

The first candidate scheme under analysis for determining the base forces is to make 
F\ and F 2 the outputs of a Schmitt trigger, as shown in Fig. 5.2. The inputs to this 
filter are the weighted sums of the errors in base position and velocity resolved in the two 
perpendicular thrust directions: 


V 


cos(g 3 ) 

sin(g 3 ) 

X K u 0 0 ‘ 

d*i 


-sin(g 3 ) 

-cos(g 3 )_ 

0 0 Kg K u 


Qldes 9l 
Hides ~ 
<l2des — 92 
.H2des ~ “ 2 . 


The parameters a, K q , and K u are chosen to yield desired response characteristics. 


5.6 Path and Motion Planning 

A path planner is required to find a viable corridor through the workspace to the desired 
terminal location. The path generated by the path planner will then be used to define a 
curvilinear coordinate system that a motion planner will use to describe the desired motion 
of the vehicles and manipulated object. In order to accomodate the possibility of unforeseen 
obstacles or a changing workspace geometry, the path will be frequently updated as the 
vehicles and manipulated object move toward their destination. 

The visibility graph (VG) [40] is the first path-planning algorithm under study. Given 
a geometric representation of the workspace, the VG method searches for the shortest path 
from an initial to final point. Fig. 5.3 shows the path found by the VG path planner in a 
workspace with several obstacles that were ’’grown” by a safety margin to prevent collisions. 
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Figure 5.3: Path found by the Visibility Graph path planner 


A motion planning algorithm has been developed that generates desired positions and 
speeds for the vehicles and object along a path found by the path planner. Where the path 
planner was concerned only with the workspace geometry, the motion planner takes into 
consideration performance limitations due to dynamic factors such as the vehicles’ mass, 
inertia, and thrust levels. Other important factors are the vehicles’ fields of view and the 
presence of unknown obstacles. 

Fig. 5.4 shows a schematic of the control loop for the multiple-vehicle manipulation 
system. 


5.7 Simulations 

Simulations of a two- vehicle object manipulation have been successfully carried out using 
the matrix manipulation program Pro-Matlab [8]. One such manipulation task, shown in 
Fig. 5.5 , utilizes two single-armed robots transporting a beam along a straight path from 
A to B. An object impedance controller assigns values of a x , a y , f x , and f y to each of the 
two robots, then the control forces and torques are determined as discussed in Section 5.5. 

At present, the path-planning algorithm has not been implemented in software, so 
Fig. 5.5 represents closed-loop control along a predefined path. However, since the method 
of generating the path is transparent to the controller, the motion of the vehicles and object 
along the predefined path will be identicle to the motion along the same path generated by 
the visibility graph or any other path planner. This independence between path planning 
and control will be convenient for developing both aspects of the closed-loop system. 


5.8 Summary 

Work has begun on the problem of multiple- vehicle cooperation. Far-range goals have 
been defined to give direction for progressing research. An initial control scheme has been 



















96 


CHAPTER 5. MULTIPLE-VEHICLE COOPERATION 


successfully simulated, and candidate path- and motion-planning algorithms are being de- 
veloped. 


5.9 Future Work 

The next major step in software development is to implement revised versions of the sim- 
ulation code in C for eventual use with our real-time system. 

The path-planning algorithm is near completion. Once the planner is operational, a 
study will be undertaken to determine the stability issues associated with the vehicles 
following a periodically changing path. 


Chapter 6 


Locomotion Enhancement via 
Arm Pushoff (LEAP) 


Warren J. Jasper 


6.1 Introduction 

To perform complex assembly tasks, an autonomous vehicle needs to move from one place 
to another. There is a high premium on the use of propellants, for every kilogram of 
propellant is provided in space only at extreme cost. Also, the use of thrusters may disturb 
the environment by impacting a target which the robot is trying to grasp. Our proposal 
for reducing substantially the use of propellant is an approach called LEAP: Locomotion 
Enhancement via Arm Pushoff. In LEAP, the vehicle pushes itself off from a large space 
object and “leaps” to the desired resting place or simply “crawls” along an object. This is 
the common mode of locomotion used by the astronauts while in the Space Shuttle. 

We believe that space robots can use this idea to good advantage. That is why this new 
project was added to investigate the problems and issues involved in autonomous space 
locomotion. The first phase of the project involves: devising the experiment, deriving the 
equations of motion and candidate control laws, and then simulating the model to size 
physical parameters for the actual experiment. The second phase encompasses design and 
fabrication of the vehicle, while the third phase is to verify experimentally the theoretical 
development. The following paragraphs describe the progress on phase two. Phase one is 
already completed. 


Progress Summary 

The major activities started or completed during the period March, 1988 through August, 
1988 were: 

• Completion of “Ballerina Bar” around the granite table. 
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• Completed assembly of high pressure plumbing. 

• Completed fabrication of major hardware (machined) components. This includes 
battery packs, battery racks, analog racks, mounting plates, posts and a variety of 
mounting brackets. About 90% of the parts have been machined for two air cushion 
vehicles. 

• Ordered digital electronics. This includes the CPU boards, A/D and D/A boards, 
and a Parallel I/O board. 

• Started assembly of low pressure plumbing, thruster subsystem, and power system. 
Also began wiring vehicle. 

6.2 The Experiment 

A new air-cushion vehicle is being designed to study LEAP. This vehicle should simulate 
the motions that an autonomous space robot would perform while in the space station or 
maneuvering out in space. The experiment will consist of the vehicle pushing off a bar 
located on one side of the granite table, rotating 180 degs, and catching itself by grasping 
a bar located at the other end of the table. Ideally, one would like to complete this task 
without the use of thrusters. However, at the point of initial release from the bar, errors 
in the velocity of the center of mass of the vehicle can only be corrected using thrusters. 
To enhance the robustness of this approach, thrusters will be incorporated into the control 
laws for midcourse correction. Figure 6.1 shows the robot in three configurations: pushing 
off the bar, rotating, and catching itself at the other end. By incorporating crawling and 
leaping, the robot can position itself anywhere on the table with a minimum amount of 
propellant. This investigation complements current work done at the Stanford Aerospace 
Robotic Laboratory [5, 7] by incorporating global navigation and object r 1 •uiipulation into 
a general study of locomotion. 
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Figure 6.1: The LEAP Demonstration 
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6.3 Fabrication 

Fabrication and assembly of two vehicles was the major emphases during the past six 
months. As mentioned in the Fourth semi-annual report [5], the overall design objective 
was to create a modular vehicle which consists of cylindrical layers. Each layer incorporates 
a major system, with five layers in all. Table 6.1 describes the systems in each layer and the 
fabrication/assembly status. This table only includes the progress/status for mechanical 
fabrication, and not wiring, testing or system integration. 

The completion of the “ballerina bar” was another interesting task. To attach the bar 
to the granite table, plastic nodules with heli-coil inserts were glued to the side of the 
table. This was by far the cheapest method and preferable to drilling holes in the side of 
the table. Unfortunately, the first attempt using Loctite 401, a cyanoacrylate, failed. This 
is because the glue was too brittle, and fractured when a torque was applied to the nodules. 
The nodules were then glued using Epoxy 2216 and tightened down with strap clamps. It 
appears that this second approach works fine. 


Task 

Layer 

Started 

Completed 

Ballerina Bar 


V 

y/ 

Robotic Arms 


V 

y/ 

High Pressure Plumbing 

I 

V 


Low Pressure Plumbing 

I 

V 


Base Plate Fabrication 

I 

y/ 

V 

Thruster System 

II 

y/ 


Momentum Wheel System 

II 

V 

yj 

Battery Packs 

III 

V 

V 

Battery Rack 

III 

V 

y/ 

Analog Rack 

III 

V 


Digital Euro Card Cage 

IV 

y/ 


Vision System 

V 




Table 6.1: Fabrication Completed 


6.4 Inertial Sensing Unit 

One area of concern was sensing the inertial position and rates of the robot base with respect 
to the inertial or lab frame. Although a vision system is being explored, it is unclear at this 
time whether velocity and acceleration values can be derived at a high enough bandwidth 
from a vision system, which essentially measures position. Acceleration data is needed to 
determine the amount of velocity or Av imparted to the robot during thruster firing, while 
angular rate information is directly used in the computed torque control law. To sense 
these quantities, an accelerometer and an angular rate sensor were acquired. The angular 
rate sensor, made by Watson Industries, is a solid state electronic device which produces 
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Power supply: 

±15 VDC ±5% 20 mA maximum 

Output: 

±10 VDC at full scale angular rate 

Sensitivity: 

±100 “/second full scale 

Output current: 

±10 mA maximum 

System frequency: 

360 Hz nominal 

Scale factor error: 

2% 

Linearity: 

< 0.1% full scale 

Frequency response: 

DC to 30 Hz 

Output noise: 

5mV RMS maximum 

Life: 

50,000 hours MTBF minimum 

Shock: 

200G 

Weight: 

110 grams 


Table 6.2: Angular Rate Sensor Specifications Model ARS-C131-1A 

an output voltage when Coriolis forces causes bending in a piezoelectric bender element. 
This rate sensor, also known as a “tuning fork gyro” has the advantages over conventional 
gyros of low cost and high reliability. Table 6.2 gives some important specifications for the 
instrument. 

The accelerometers used on the LEAP vehicle are two Systron Donner 4310 liner servo 
accelerometers. As Table 6.3 shows, these are high quality instruments. The output will 
be filtered for scale factor and bias, and integrated twice to give velocity and position. At 
this time, it is unclear whether the integration will be done in hardware or software. 

One of the challenges will be to detect acceleration on the order of O.lmg to lOmg 
in a lg environment. Although accelerations in the x and y directions are orthogonal and 
theoretically decoupled from each other and the lg gravity force in the z axes, there is some 
cross axis coupling on the order of <0.002g/g of applied acceleration. The major source 
of error will occur in detecting these small accelerations in a lg environment. By way of 
example, if the accelerometers are set to read lOmg full scale, a tilt in the base of 20 arc 
seconds (which corresponds to a change in height of the base with respect to the granite 
table of 0.0009 inches) causes an error of 1% or O.lmg. This will only become a problem if 
one integrates acceleration twice to derive position, for errors in acceleration grow as t 2 in 
position. 
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Power supply: 

±15 VDC ±10% at 10 mA maximum 

Output: 

±7.5 VDC at full range 

Sensitivity: 

±1 g full scale 

Output current: 

±3 mA maximum 

Zero Output (Null): 

< 0.05% full range 

Linearity: 

< 0.05% full scale 

Natural frequency: 

50 - 250 Hz 

Output noise: 

< 7.5m V RMS 

Resolution 

< 0.001% full range 

Shock: 

100G 11msec 

Weight: 

128 grams 


Table 6.3: Linear Servo Accelerometer Model 4310 

6.5 Future Work 

The robotic vehicles should be near completion in the next five months. The major tasks 
yet to be completed are wiring, system integration and test. Fortunately, many of these 
tasks can be done in parallel and so experiments should begin by the summer of 1989. 



Chapter 7 


Adaptive Control of LEAP 


Roberto Ernesto Zanutta 


7.1 Introduction 

A major task of free-flying robots is to aid in the construction, maintenance and repair of 
space structures (e.g. the space station and satellites) while in orbit. Because of the high 
costs of placing mass into orbit, it is desirable to reduce the amount of propellant consumed 
by the free-flying robots while carrying out their tasks. Typical tasks performed by the 
robots will require them to transport and retrieve various objects. To minimize the amount 
of propellant required, the robots will have to know accurately the inertia properties of 
the objects they carry. Since it is not practical to specify the object mass properties each 
time a robot performs a task, some method of identification is necessary. This can be done 
through the use of adaptive control. 

The investigation of adaptive control for a two-armed free-flying robot is a recent project. 
The previously reported work has been in the investigation of adaptive control schemes and 
vehicle modeling and simulation. A major part of this was a literature search. The following 
is a summary of the work done on the project during the last six months. This work consists 
mainly of control law and adaptation law simulation and analysis. Also included is a brief 
description of future work. 

7.2 Control Law Development 

The first step in the development of an adaptive controller for the vehicle is the determina- 
tion of a suitable control law. This has been the main thrust of the recent research on this 
project. As was reported in the previous semi-annual report two approaches were found in 
the literature which had desireable characteristics for real-time applications. These were 
presented by Slotine and Li of MIT [34] and Wen and Bayard of JPL [4]. These approaches 
must be extended and modified for implementation on a two-armed free-flying robot. The 
following is a description of the modifications made and preliminary simulation results. 


preceding page blank not filmed 
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The control law chosen to command the robot is one proposed by Slotine and Li [34]. 


It is: 


r = M(q)q r + C(q,q)q r -K v s 

(7.1) 

where 


qr = q d - Aq 

(7.2) 


q = q - qa 

(7.3) 


s = q-qd + A(q-q d ) 

(7.4) 


A and K v are positive definite diagonal matrices, q is the vector of the system states 
(vehicle position and arm orientations), q j is the vector of desired states, q is the vector 
of tracking errors, r is the vector of forces and torques associated with the system states 
(applied forces and torques). M is the estimated system mass matrix. C(q, q)qr is the 
estimated system non-linear terms. 

This control law does not consider the closed-loop kinematic condition that occurs when 
the robot is holding an object with both arms. To deal with this problem the non-holonomic 
equations of motion were used [18]. 

The control law was modified by treating the sliding mode term (7.4) as an applied force 
and incorporating the closed-loop kinematic constraints (see Appendix C of the Fifth Semi- 
Annual Report) forming the non-holonomic equations of motion. The resulting control law 
is: 

r a + A^ 3 t t = C 8 — K v s s + Aj 3 (C r — K v sr) 4- (M sr + A^ a M r r)Br 

+ (Mgs + Mg r A ra + A* 3 Mrs + A^M rr A r *)u s (7.5) 

The s and r subscripts refer to the independent and dependent degrees of freedom respec- 
tively. 

The choice of independent degrees of freedom is fairly arbitrary. The independent 
degrees of freedom were chosen to be the base orientation, shoulder angles and momentum 
wheel orientation (s = 3, 4, 6, 8). This choice results in the applied torque distribution: 


7g + A^r r 


03175 - 1 - 0417-7 \ 

74 + ® 32 7*5 + 0427-7 
76 + 0337"5 + 0437-7 
\ T& ) 


(7.6) 


These equations provide the relationship between the torques required to achieve the 
desired motion, but do not give a unique solution. There is still some freedom in how the 
torques can be distributed. 

As a first cut the two shoulder motor torques were set equal. Simulation results using 
the control law (7.1) are shown on the next page. The graphs show the error between the 
desired and actual position of the left shoulder and elbow motors (tracking error). The 
tracking errors are less than 0.03 degrees. Similar results were obtained for the right arm 
and momentum wheel. These results demonstrate that, under ideal conditions the control 
law works well. 



angle error (deg) angle error (deg) 
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Figure 7.1: Tracking error using the control law 
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7.3 Adaptation Law 

The control law and adaptation laws are closely related. The first adaptation law simulated 
is also one proposed by Slotine and Li. The adaptation law is: 

a=_r- 1 Y T (q,q,q r ,q r )4r (7-7) 

This law has some strong and weak points. Its main advantage is that it is very simple to 
implement using the control law (7.1). It uses the tracking error which is readily available 
and therefore adds to the simplicity of the algorithm. At the same time tracking error 
presents a problem in identification because it is small when good tracking is achieved, 
regardless of the parameter estimation. As a result when the desired trajectory is accurately 
tracked the parameter estimates will not change even when they are off the true values. 
In order to obtain a good parameter estimation a rough "sufficiently exciting” trajectory 
is desired. This presents a conflict with good tracking which requires smooth trajectories. 
Because of this conflict and the importance of good parameter identification a two-phase 
approach appears desireable. In the first phase the robot will ’’shake” itself about to 
determine its inertial parameters. During the second phase the robot will perform the 
desired task using a smooth trajectory. 

This adaptation law with the control law (7.1) was simulated. Various trajectories and 
control parameters were tried. Two cases are presented: the first when adapting to one 
parameter (total mass) and the second when adapting to eight parameters (the minimum 
number when adding a mass to the base). The trajectory used for adaptation was generated 
using filtered random torques for the actuators. This generated a trajectory which was 
’’exciting” and therefore good for identification. 

As can be seen the scheme worked well when adapting to one parameter, but failed 
when adapting to eight parameters. In both cases the tracking error was small (less than 
2.5 degrees). But, in the eight parameter case the parameters did not adapt well. The 
following figure shows the mass estimates to be off by more than an order of magnitude. 
The motors saturated when large adaptation gain values were used to try to improve the 
performance of the adaptation law. Once the motors saturate little, if any improvement is 
achievable. 

More effort is required to improve this adaptation scheme. But, considering the past 
results there appears little hope that the scheme will yield promising results. An alternate 
method is presently being investigated. This new method will require either more hardware 
or a more complicated adaptation law. 


7.4 Adaptation Law II 

A new approach for adaptation, as suggested by Li and Slotine [25] is being considered. 
The method uses prediction error in addition to tracking error to drive the adaptation 
process. As a result twice as much information is used during the identification phase. The 
method has some very desireable properties. The approach has exponential convergence 
for persistently exciting trajectories and guarantees bounded parameter estimates. The 
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method is based on the standard least-squares and exponential forgetting least-squares 
methods. But, as in the case of the control law, modifications have to be made. 

There are two approaches which are being considered for modifying the adaptation law. 
The first is to use force sensors at the robot arm tips and to treat the system constraints 
only through these forces. The second approach uses the constrained system equations 
directly in the identification scheme. This latter method does not require force sensors, 
but the unknown parameters appear as non-linear terms in the control and identification 
equations. This will slow down the rate of convergence and may even prevent parameter 
convergence. The necessary modifications have been made to the algorithm for simulation. 

7.5 Future Work 

The simulation studies on the adaptation laws must be completed. These studies will 
determine the next step of research since it may be necessary to add force sensors to the 
arm tips. Upon completion of the simulation studies the real-time implementation aspects 
of the chosen algorithm will be addressed. Ongoing work in the hardware development will 
continue. 
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